I want to convert coordinates from the JSON files to ROS coordinate conventions and a system with origin in CAM_A.
I recursively walk the extrinsics tree until I get to CAM_A in order to compute the coordinate of each cam and imu (the walk starts at leaves and goes towards the root).
Regarding translation, everything works. However, I can't get the rotation part working for the IMUs.
First, please confirm that the coordinate system used in the JSONs is as follows:
- X left
- Y up
- Z forward (along optical axis of camera)
ROS coords follow:
So, transforming JSON coords to ROS coords is just a simple shuffle of the coords and multiplication by 0.01 to convert to meters.
As an example, I took OAK-D-PRO-POE and opened in CAD.
From this, I get that the BNO IMU position should be approx (-4.0, -2.0, -0.2) on the JSON frame and (-0.002, -0.04, -0.02) in ROS frame. I also know the IMU is facing along negative Z, so there should be yaw or pitch 180 degrees in ROS.
However, the JSON has these extrinsics:
CAM_C: (3.75, 0, 0) to CAM_A -> global coord (-3.75, 0, 0)
CAM_B: (-7.5, 0, 0) to CAM_C -> global coord (3.75, 0, 0)
BNO: (7.75, -0.2, 2.0264) + RPY (180, 0, 90) to CAM_B -> global coord (-4.0, 0.2, -2.0264)
I see that this computed BNO position doesn't fit (X is fine, but Y and Z are swapped and negated), so I guess the extrinsic rotation has to be applied to the offset vector. However, no variant I've tried led to the correct result (and I've tried all of them, including different Euler angle interpretations). I do even have the bad feeling that these two results do not differ by a rotation, but a reflection...
So either I do something fundamentally wrong in my computations, or the data in the JSON are wrong.
This is the code I use to test this conversion:
#!/usr/bin/env python3
"""
This script can read a JSON from https://github.com/luxonis/depthai-boards/blob/main/boards and converts it to a format
that is suitable for urdf/cameras/luxonis.yaml in this repo. The update has to be manual, though.
Generated by: Gemini 3 (thinking mode)
Manually postedited by Martin Pecka
"""
import json
import numpy as np
import sys
from scipy.spatial.transform import Rotation as R
from tf.transformations import quaternion_from_euler
try:
from_matrix = R.from_matrix
as_matrix = R.as_matrix
except:
from_matrix = R.from_dcm
as_matrix = R.as_dcm
def get_rotation_from_json(rot_dict):
"""Helper to create a SciPy Rotation object from JSON r,p,y."""
# I've tried lots of things here
return R.from_euler('xyz', [rot_dict['r'], rot_dict['p'], rot_dict['y']], degrees=True)
def resolve_extrinsics(sensor_name, cameras_data, imu_data=None):
"""
Recursively finds the absolute position and rotation relative to the root (origin).
The extrinsics 'specTranslation' describes translation FROM child TO parent.
So we need to negate it to get child position relative to parent.
The rotation describes the orientation of the child frame relative to parent.
"""
# Base Case: If this is CAM_A (the origin), return identity
if sensor_name == "CAM_A":
return np.zeros(3), R.from_quat([0, 0, 0, 1])
# Determine if we are looking at a camera or the IMU
if imu_data is not None and sensor_name in imu_data:
cfg = imu_data[sensor_name]['extrinsics']
elif sensor_name in cameras_data:
cfg = cameras_data[sensor_name].get('extrinsics')
if not cfg: # Camera exists but has no extrinsics (likely the origin)
return np.zeros(3), R.from_quat([0, 0, 0, 1])
else:
raise ValueError(f"Sensor {sensor_name} not found in config.")
parent_name = cfg['to_cam']
parent_pos, parent_rot_global = resolve_extrinsics(parent_name, cameras_data, imu_data)
spec_trans = cfg['specTranslation']
local_rot = get_rotation_from_json(cfg['rotation'])
# The specTranslation is expressed in the child's rotated frame
# We need to rotate it to the parent frame first, then negate it
local_pos = -np.array([spec_trans['x'], spec_trans['y'], spec_trans['z']])
# I've also tried lots of things along these lines
# Apply parent rotation to the local position, then add to parent position
absolute_pos = parent_pos + parent_rot_global.apply(local_pos)
# Compose rotations: child orientation = parent_orientation * local_rotation
absolute_rot = parent_rot_global * local_rot
return absolute_pos, absolute_rot
def convert_oak_universal(json_data):
data = json_data['board_config']
cams = data['cameras']
imus = data.get('imuExtrinsics', {}).get('sensors', {})
stereo = data.get('stereo_config', None)
# Coordinate system mapping between Luxonis frame and ROS geometric frame
# Luxonis Frame: X:left, Y:up, Z:forward
# ROS Frame: X:forward, Y:left, Z:up
M = np.array([
[0, 0, 1], # ROS X (forward) = Luxonis Z
[1, 0, 0], # ROS Y (left) = Luxonis X
[0, 1, 0] # ROS Z (up) = Luxonis Y
])
def to_user_m(p_j):
return M @ p_j / 100
def format_angle(a):
return f"{a:.4f}"
def format_num(n):
if np.allclose(n, 0):
return '0.0'
return f"{n:.3f}".rstrip("0")
def format_offset(o):
return f"x: {format_num(o[0])}, y: {format_num(o[1])}, z: {format_num(o[2])}"
def get_sensor_name(socket):
if socket in cams:
name = cams[socket]['name']
if name in ('color', 'center'):
name = 'rgb'
else:
name = imus[socket]['name']
return name
def mag_sensor(imu_sensor):
if imu_sensor.startswith("BNO"):
return imu_sensor
elif imu_sensor.startswith("BMI"):
return None
elif imu_sensor.startswith("ICM-42"):
return "AK09919C"
return None
all_sensors = list(cams.keys()) + list(imus.keys())
all_sensor_names = [(get_sensor_name(name), name) for name in all_sensors]
transforms = {}
for sensor_name, socket_name in sorted(all_sensor_names):
pos_j, rot_j = resolve_extrinsics(socket_name, cams, imus)
pos_u = to_user_m(pos_j)
r_u_mat = M @ as_matrix(rot_j) @ M.T
r_u = from_matrix(r_u_mat)
euler_u = r_u.as_euler('xyz')
if socket_name in cams:
offset = format_offset(pos_u)
print(f"'{sensor_name}': {{sensor_type: '', hfov: {cams[socket_name]['hfov']:.1f}, {offset}, socket: '{socket_name}'}}")
transforms[socket_name] = pos_u, euler_u
if stereo is not None:
print(f"'stereo': {{left_cam: '{stereo['left_cam']}', right_cam: '{stereo['right_cam']}'}}")
# Append IMUs
for socket_name in imus.keys():
pos, rot = transforms[socket_name]
# pos = [-pos[2], pos[1], pos[0]] # TODO: BAD BAD fix this should definitely not be here
mag_sensor_name = mag_sensor(get_sensor_name(socket_name))
mag = f", mag_sensor: '{mag_sensor_name}'" if mag_sensor_name is not None else ''
offset = format_offset(pos)
rpy = f"R: {format_angle(rot[0])}, P: {format_angle(rot[1])}, Y: {format_angle(rot[2])}"
print(f"'imu': {{imu_sensor: '{get_sensor_name(socket_name)}'{mag}, {offset}, {rpy}}}")
if __name__ == "__main__":
test_json = """
{
"board_config":
{
"name": "OAK-D-PRO-POE",
"revision": "R3M1E3",
"cameras":{
"CAM_A": {
"name": "color",
"hfov": 68.7938,
"type": "color"
},
"CAM_B": {
"name": "left",
"hfov": 80.0,
"type": "mono",
"extrinsics": {
"to_cam": "CAM_C",
"specTranslation": {
"x": -7.5,
"y": 0,
"z": 0
},
"rotation":{
"r": 0,
"p": 0,
"y": 0
}
}
},
"CAM_C": {
"name": "right",
"hfov": 71.86,
"type": "mono",
"extrinsics": {
"to_cam": "CAM_A",
"specTranslation": {
"x": 3.75,
"y": 0,
"z": 0
},
"rotation":{
"r": 0,
"p": 0,
"y": 0
}
}
}
},
"stereo_config":{
"left_cam": "CAM_B",
"right_cam": "CAM_C"
},
"imuExtrinsics":
{
"sensors":{
"BNO": {
"name" : "BNO086",
"extrinsics": {
"to_cam": "CAM_B",
"specTranslation": {
"x": 7.75,
"y": -0.2,
"z": 2.0264
},
"rotation":{
"r": 180,
"p": 0,
"y": 90
}
}
},
"BMI": {
"name" : "BMI270",
"extrinsics": {
"to_cam": "CAM_B",
"specTranslation": {
"x": 8.5475,
"y": -0.2,
"z": 1.646
},
"rotation":{
"r": 180,
"p": 0,
"y": 270
}
}
}
}
}
}
}
"""
if len(sys.argv) > 1:
with open(sys.argv[1], 'r') as f:
oak_json = json.load(f)
else:
oak_json = json.loads(test_json)
convert_oak_universal(oak_json)This code currently returns:
'left': {sensor_type: '', hfov: 80.0, x: 0.0, y: 0.037, z: 0.0, socket: 'CAM_B'}
'rgb': {sensor_type: '', hfov: 68.8, x: 0.0, y: 0.0, z: 0.0, socket: 'CAM_A'}
'right': {sensor_type: '', hfov: 71.9, x: 0.0, y: -0.037, z: 0.0, socket: 'CAM_C'}
'stereo': {left_cam: 'CAM_B', right_cam: 'CAM_C'}
'imu': {imu_sensor: 'BNO086', mag_sensor: 'BNO086', x: -0.02, y: -0.04, z: 0.002, R: 1.5708, P: 0.0000, Y: 3.1416}
'imu': {imu_sensor: 'BMI270', x: -0.016, y: -0.048, z: 0.002, R: -1.5708, P: 0.0000, Y: -3.1416}
You can see that the BNO offset is wrong.
I want to convert coordinates from the JSON files to ROS coordinate conventions and a system with origin in CAM_A.
I recursively walk the extrinsics tree until I get to CAM_A in order to compute the coordinate of each cam and imu (the walk starts at leaves and goes towards the root).
Regarding translation, everything works. However, I can't get the rotation part working for the IMUs.
First, please confirm that the coordinate system used in the JSONs is as follows:
ROS coords follow:
So, transforming JSON coords to ROS coords is just a simple shuffle of the coords and multiplication by 0.01 to convert to meters.
As an example, I took OAK-D-PRO-POE and opened in CAD.
From this, I get that the BNO IMU position should be approx (-4.0, -2.0, -0.2) on the JSON frame and (-0.002, -0.04, -0.02) in ROS frame. I also know the IMU is facing along negative Z, so there should be yaw or pitch 180 degrees in ROS.
However, the JSON has these extrinsics:
CAM_C: (3.75, 0, 0) to CAM_A -> global coord (-3.75, 0, 0)
CAM_B: (-7.5, 0, 0) to CAM_C -> global coord (3.75, 0, 0)
BNO: (7.75, -0.2, 2.0264) + RPY (180, 0, 90) to CAM_B -> global coord (-4.0, 0.2, -2.0264)
I see that this computed BNO position doesn't fit (X is fine, but Y and Z are swapped and negated), so I guess the extrinsic rotation has to be applied to the offset vector. However, no variant I've tried led to the correct result (and I've tried all of them, including different Euler angle interpretations). I do even have the bad feeling that these two results do not differ by a rotation, but a reflection...
So either I do something fundamentally wrong in my computations, or the data in the JSON are wrong.
This is the code I use to test this conversion:
This code currently returns:
You can see that the BNO offset is wrong.