intel realsense d435i相机标定中文文档
-
此文档参考了官方的英文文档,原地址面向英特尔®实感™深度摄像头的 IMU 校准工具 (intelrealsense.com)
-
IMU概述:惯性测量单元(imu)通常由加速度计组成,加速度通常以国际系统(SI)的米/秒为单位输出平方(m/s^2)和陀螺仪,陀螺仪通常以SI单位测量角速度弧度/秒(rad/s)。英特尔RealSense™深度相机中的IMU D435i和D455相机和英特尔®RealSense™激光雷达相机L515没有什么不同和包含加速度计和陀螺仪可配置输出频率。
-
IMU校准参数:IMU标定参数包括内部参数和外部参数。虽然有许多可能的IMU校准参数,为了简单起见我们考虑一下以下参数:
内在参数试图通过对传感器的不准确性进行建模,将原始传感器数据转换为实际测量值。加速度计的数学变换如下:
在这种情况下,陀螺仪遵循以下简单的模型:
有关IMU ininic参数的基础知识,请参阅意法半导体(STMicroelectronics):pdf地址
外部参数包括:
- Rotation -从左红外(IR)相机(IR1)到IMU的旋转,指定为3 × 3旋转矩阵
- Translation -从左侧IR相机到IMU的平移量,指定为3 × 1矢量,单位为毫米
本文件的范围仅包括内部参数校准过程。外部参数可用libaresense。
IMU校准工具
在Intel®RealSense™SDK 2.0中包含了一个校准python脚本rs-imu-calibration.py。请稍后在本文档中查看详细信息。以D435i、D455和L515摄像机为例,介绍了IMU的标定过程。
局限性和准确性
虽然所提供的工具取得了良好的整体校准结果,但如果不遵循所有步骤,则校准将不准确。下列因素亦可能影响校正效果:
用户自定义校准
如果用户更喜欢使用自己的工具校准设备以满足特定的应用需求,结果可以按照rs-imu- calibration .py中演示的方法写入设备。
IMU校准表格式请参见pdf表格
表的格式在所有设备上都是相同的,除了表头中的version和table type字段。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-q2LwDV23-1680153837913)(C:\Users\shogoki\AppData\Roaming\Typora\typora-user-images\image-20230330113004310.png)]
FW 要求
安装
本节介绍运行校准Python脚本校准设备所需的硬件和软件设置。
硬件
所需硬件包括待校准的D435i、D455或L515设备、一根USB线缆、一台运行Windows 10、Ubuntu 16.04和Ubuntu 18.04的计算机。
-
设备
- 使用如下所示的Intel®RealSense™深度相机D435i, Intel®RealSense™深度相机D455,或Intel®RealSense™LiDAR相机L515设备来显示校准过程。
-
USB
-
PC
软件:在主机上安装以下软件
- Python On Windows: Download and install Python: https://www.python.org/downloads/windows/
- Python校准脚本:校准脚本(Intel®RealSense™SDK的一部分)rs-imu-calibration.py,可在https://github.com/IntelRealSense/librealsense上获得。请下载最新版本,以确保支持所有校准功能。该文件位于源代码树的tools/rs-imu-calibration目录中。
from __future__ import print_function
import numpy as np
import sys
import json
import ctypes
import os
import binascii
import struct
import pyrealsense2 as rs
import ctypes
import time
import enum
import threading
READ_TABLE = 0x43
WRITE_TABLE = 0x44
L515_FW_VER_REQUIRED = '01.04.01.00'
is_data = None
get_key = None
if os.name == 'posix':
import select
import tty
import termios
is_data = lambda: select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
get_key = lambda: sys.stdin.read(1)
elif os.name == 'nt':
import msvcrt
is_data = msvcrt.kbhit
get_key = lambda: msvcrt.getch()
else:
raise Exception('Unsupported OS: %s' % os.name)
if sys.version_info[0] < 3:
input = raw_input
max_float = struct.unpack('f', b'\xff\xff\xff\xff')[0]
max_int = struct.unpack('i', b'\xff\xff\xff\xff')[0]
max_uint8 = struct.unpack('B', b'\xff')[0]
g = 9.80665
COLOR_RED = "\033[1;31m"
COLOR_BLUE = "\033[1;34m"
COLOR_CYAN = "\033[1;36m"
COLOR_GREEN = "\033[0;32m"
COLOR_RESET = "\033[0;0m"
COLOR_BOLD = "\033[;1m"
COLOR_REVERSE = "\033[;7m"
def int_to_bytes(num, length=4, order='big'):
res = bytearray(length)
for i in range(length):
res[i] = num & 0xff
num >>= 8
if num:
raise OverflowError("Number {} doesn't fit into {} bytes.".format(num, length))
if order == 'little':
res.reverse()
return res
def bytes_to_uint(bytes_array, order='little'):
bytes_array = list(bytes_array)
bytes_array.reverse()
if order == 'little':
return struct.unpack('>i', struct.pack('BBBB', *([0] * (4 - len(bytes_array))) + bytes_array))[0] & 0xffffffff
else:
return struct.unpack('>i', struct.pack('BBBB', *([0] * (4 - len(bytes_array))) + bytes_array))[0] & 0xffffffff
class imu_wrapper:
class Status(enum.Enum):
idle = 0,
rotate = 1,
wait_to_stable = 2,
collect_data = 3
def __init__(self):
self.pipeline = None
self.imu_sensor = None
self.status = self.Status(
self.Status.idle)
self.thread = threading.Condition()
self.step_start_time = time.time()
self.time_to_stable = 3
self.time_to_collect = 2
self.samples_to_collect = 1000
self.rotating_threshold = 0.1
self.moving_threshold_factor = 0.1
self.collected_data_gyro = []
self.collected_data_accel = []
self.callback_lock = threading.Lock()
self.max_norm = np.linalg.norm(np.array([0.5, 0.5, 0.5]))
self.line_length = 20
self.is_done = False
self.is_data = False
def escape_handler(self):
self.thread.acquire()
self.status = self.Status.idle
self.is_done = True
self.thread.notify()
self.thread.release()
sys.exit(-1)
def imu_callback(self, frame):
if not self.is_data:
self.is_data = True
with self.callback_lock:
try:
if is_data():
c = get_key()
if c == '\x1b':
self.escape_handler()
if self.status == self.Status.idle:
return
pr = frame.get_profile()
data = frame.as_motion_frame().get_motion_data()
data_np = np.array([data.x, data.y, data.z])
elapsed_time = time.time() - self.step_start_time
if self.status == self.Status.collect_data:
sys.stdout.write('\r %15s' % self.status)
part_done = len(self.collected_data_accel) / float(self.samples_to_collect)
color = COLOR_GREEN
if pr.stream_type() == rs.stream.gyro:
self.collected_data_gyro.append(np.append(frame.get_timestamp(), data_np))
is_moving = any(abs(data_np) > self.rotating_threshold)
else:
is_in_norm = np.linalg.norm(data_np - self.crnt_bucket) < self.max_norm
if is_in_norm:
self.collected_data_accel.append(np.append(frame.get_timestamp(), data_np))
else:
color = COLOR_RED
is_moving = abs(np.linalg.norm(data_np) - g) / g > self.moving_threshold_factor
sys.stdout.write(color)
sys.stdout.write('[' + '.' * int(part_done * self.line_length) + ' ' * int(
(1 - part_done) * self.line_length) + ']')
sys.stdout.write(COLOR_RESET)
if is_moving:
print('WARNING: MOVING')
self.status = self.Status.rotate
return
if part_done >= 1:
self.status = self.Status.collect_data
sys.stdout.write('\n\nDirection data collected.')
self.thread.acquire()
self.status = self.Status.idle
self.thread.notify()
self.thread.release()
return
if pr.stream_type() == rs.stream.gyro:
return
sys.stdout.write('\r %15s' % self.status)
crnt_dir = np.array(data_np) / np.linalg.norm(data_np)
crnt_diff = self.crnt_direction - crnt_dir
is_in_norm = np.linalg.norm(data_np - self.crnt_bucket) < self.max_norm
if self.status == self.Status.rotate:
sys.stdout.write(': %35s' % (np.array2string(crnt_diff, precision=4, suppress_small=True)))
sys.stdout.write(': %35s' % (np.array2string(abs(crnt_diff) < 0.1)))
if is_in_norm:
self.status = self.Status.wait_to_stable
sys.stdout.write('\r' + ' ' * 90)
self.step_start_time = time.time()
return
if self.status == self.Status.wait_to_stable:
sys.stdout.write(': %-3.1f (secs)' % (self.time_to_stable - elapsed_time))
if not is_in_norm:
self.status = self.Status.rotate
return
if elapsed_time > self.time_to_stable:
self.collected_data_gyro = []
self.collected_data_accel = []
self.status = self.Status.collect_data
self.step_start_time = time.time()
return
return
except Exception as e:
print('ERROR?' + str(e))
self.thread.acquire()
self.status = self.Status.idle
self.thread.notify()
self.thread.release()
def get_measurements(self, buckets, bucket_labels):
measurements = []
print('-------------------------')
print('*** Press ESC to Quit ***')
print('-------------------------')
for bucket, bucket_label in zip(buckets, bucket_labels):
self.crnt_bucket = np.array(bucket)
self.crnt_direction = np.array(bucket) / np.linalg.norm(np.array(bucket))
print('\nAlign to direction: ', self.crnt_direction, ' ', bucket_label)
self.status = self.Status.rotate
self.thread.acquire()
while (not self.is_done and self.status != self.Status.idle):
self.thread.wait(3)
if not self.is_data:
raise Exception('No IMU data. Check connectivity.')
if self.is_done:
raise Exception('User Abort.')
measurements.append(np.array(self.collected_data_accel))
return np.array(measurements), np.array(self.collected_data_gyro)
def enable_imu_device(self, serial_no):
self.pipeline = rs.pipeline()
cfg = rs.config()
cfg.enable_device(serial_no)
try:
self.pipeline.start(cfg)
except Exception as e:
print('ERROR: ', str(e))
return False
active_imu_profiles = []
active_profiles = dict()
self.imu_sensor = None
for sensor in self.pipeline.get_active_profile().get_device().sensors:
for pr in sensor.get_stream_profiles():
if pr.stream_type() == rs.stream.gyro and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if pr.stream_type() == rs.stream.accel and pr.format() == rs.format.motion_xyz32f:
active_profiles[pr.stream_type()] = pr
self.imu_sensor = sensor
if self.imu_sensor:
break
if not self.imu_sensor:
print('No IMU sensor found.')
return False
print('\n'.join(['FOUND %s with fps=%s' % (str(ap[0]).split('.')[1].upper(), ap[1].fps()) for ap in
active_profiles.items()]))
active_imu_profiles = list(active_profiles.values())
if len(active_imu_profiles) < 2:
print('Not all IMU streams found.')
return False
self.imu_sensor.stop()
self.imu_sensor.close()
self.imu_sensor.open(active_imu_profiles)
self.imu_start_loop_time = time.time()
self.imu_sensor.start(self.imu_callback)
if self.imu_sensor.supports(rs.option.enable_motion_correction):
self.imu_sensor.set_option(rs.option.enable_motion_correction, 0)
return True
class CHeader:
def __init__(self, version, table_type):
self.buffer = np.ones(16, dtype=np.uint8) * 255
self.buffer[0] = int(version[0], 16)
self.buffer[1] = int(version[1], 16)
self.buffer.dtype = np.uint16
self.buffer[1] = int(table_type, 16)
def size(self):
return 16
def set_data_size(self, size):
self.buffer.dtype = np.uint32
self.buffer[1] = size
def set_crc32(self, crc32):
self.buffer.dtype = np.uint32
self.buffer[3] = crc32 % (1 << 32)
def get_buffer(self):
self.buffer.dtype = np.uint8
return self.buffer
def bitwise_int_to_float(ival):
return struct.unpack('f', struct.pack('i', ival))[0]
def bitwise_float_to_int(fval):
return struct.unpack('i', struct.pack('f', fval))[0]
def parse_buffer(buffer):
cmd_size = 24
header_size = 16
buffer.dtype = np.uint32
tab1_size = buffer[3]
buffer.dtype = np.uint8
print('tab1_size (all_data): ', tab1_size)
tab1 = buffer[cmd_size:cmd_size + tab1_size]
tab1.dtype = np.uint32
tab2_size = tab1[1]
tab1.dtype = np.uint8
print('tab2_size (calibration_table): ', tab2_size)
tab2 = tab1[header_size:header_size + tab2_size]
tab2.dtype = np.uint32
tab3_size = tab2[1]
tab2.dtype = np.uint8
print('tab3_size (calibration_table): ', tab3_size)
tab3 = tab2[header_size:header_size + tab3_size]
tab3.dtype = np.uint32
tab4_size = tab3[1]
tab3.dtype = np.uint8
print('tab4_size (D435_IMU_Calib_Table): ', tab4_size)
tab4 = tab3[header_size:header_size + tab4_size]
return tab1, tab2, tab3, tab4
def get_IMU_Calib_Table(X, product_line):
version = ['0x02', '0x01']
table_type = '0x20'
if product_line == 'L500':
version = ['0x05', '0x01']
table_type = '0x243'
header = CHeader(version, table_type)
header_size = header.size()
data_size = 37 * 4 + 96
size_of_buffer = header_size + data_size
assert (size_of_buffer % 4 == 0)
buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255
use_extrinsics = False
use_intrinsics = True
data_buffer = np.ones(data_size, dtype=np.uint8) * 255
data_buffer.dtype = np.float32
data_buffer[0] = bitwise_int_to_float(np.int32(int(use_intrinsics)) << 8 |
np.int32(int(use_extrinsics)))
intrinsic_vector = np.zeros(24, dtype=np.float32)
intrinsic_vector[:9] = X[:3, :3].T.flatten()
intrinsic_vector[9:12] = X[:3, 3]
intrinsic_vector[12:21] = X[3:, :3].flatten()
intrinsic_vector[21:24] = X[3:, 3]
data_buffer[13:13 + X.size] = intrinsic_vector
data_buffer.dtype = np.uint8
header.set_data_size(data_size)
header.set_crc32(binascii.crc32(data_buffer))
buffer[:header_size] = header.get_buffer()
buffer[header_size:] = data_buffer
return buffer
def get_calibration_table(d435_imu_calib_table):
version = ['0x02', '0x00']
table_type = '0x20'
header = CHeader(version, table_type)
d435_imu_calib_table_size = d435_imu_calib_table.size
sn_table_size = 32
data_size = d435_imu_calib_table_size + sn_table_size
header_size = header.size()
size_of_buffer = header_size + data_size
assert (size_of_buffer % 4 == 0)
buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255
data_buffer = np.ones(data_size, dtype=np.uint8) * 255
data_buffer[:d435_imu_calib_table_size] = d435_imu_calib_table
header.set_data_size(data_size)
header.set_crc32(binascii.crc32(data_buffer))
buffer[:header_size] = header.get_buffer()
buffer[header_size:header_size + data_size] = data_buffer
return buffer
def get_eeprom(calibration_table):
version = ['0x01', '0x01']
table_type = '0x09'
header = CHeader(version, table_type)
DC_MM_EEPROM_SIZE = 520
header_size = header.size()
size_of_buffer = DC_MM_EEPROM_SIZE
data_size = size_of_buffer - header_size
assert (size_of_buffer % 4 == 0)
buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255
header.set_data_size(data_size)
buffer[header_size:header_size + calibration_table.size] = calibration_table
header.set_crc32(binascii.crc32(buffer[header_size:]))
buffer[:header_size] = header.get_buffer()
return buffer
def write_eeprom_to_camera(eeprom, serial_no=''):
DC_MM_EEPROM_SIZE = eeprom.size
DS5_CMD_LENGTH = 24
MMEW_Cmd_bytes = b'\x14\x00\xab\xcd\x50\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'
buffer = np.ones([DC_MM_EEPROM_SIZE + DS5_CMD_LENGTH, ], dtype=np.uint8) * 255
cmd = np.array(struct.unpack('I' * 6, MMEW_Cmd_bytes), dtype=np.uint32)
cmd.dtype = np.uint16
cmd[0] += DC_MM_EEPROM_SIZE
cmd.dtype = np.uint32
cmd[3] = DC_MM_EEPROM_SIZE
cmd.dtype = np.uint8
buffer[:len(cmd)] = cmd
buffer[len(cmd):len(cmd) + eeprom.size] = eeprom
debug = get_debug_device(serial_no)
if not debug:
print('Error getting RealSense Device.')
return
rcvBuf = debug.send_and_receive_raw_data(bytearray(buffer))
if rcvBuf[0] == buffer[4]:
print('SUCCESS: saved calibration to camera.')
else:
print('FAILED: failed to save calibration to camera.')
print(rcvBuf)
def get_debug_device(serial_no):
ctx = rs.context()
devices = ctx.query_devices()
found_dev = False
for dev in devices:
if len(serial_no) == 0 or serial_no == dev.get_info(rs.camera_info.serial_number):
found_dev = True
break
if not found_dev:
print('No RealSense device found' + str('.' if len(serial_no) == 0 else ' with serial number: ' + serial_no))
return 0
print(' Device PID: ', dev.get_info(rs.camera_info.product_id))
print(' Device name: ', dev.get_info(rs.camera_info.name))
print(' Serial number: ', dev.get_info(rs.camera_info.serial_number))
print(' Firmware version: ', dev.get_info(rs.camera_info.firmware_version))
debug = rs.debug_protocol(dev)
return debug
def check_X(X, accel, show_graph):
fdata = np.apply_along_axis(np.dot, 1, accel, X[:3, :3]) - X[3, :]
norm_data = (accel ** 2).sum(axis=1) ** (1. / 2)
norm_fdata = (fdata ** 2).sum(axis=1) ** (1. / 2)
if show_graph:
import pylab
pylab.plot(norm_data, '.b')
pylab.plot(norm_fdata, '.g')
pylab.show()
print('norm (raw data ): %f' % np.mean(norm_data))
print('norm (fixed data): %f' % np.mean(norm_fdata), "A good calibration will be near %f" % g)
def l500_send_command(dev, op_code, param1=0, param2=0, param3=0, param4=0, data=[], retries=1):
for i in range(retries):
try:
debug_device = rs.debug_protocol(dev)
gvd_command_length = 0x14 + len(data)
magic_number1 = 0xab
magic_number2 = 0xcd
buf = bytearray()
buf += bytes(int_to_bytes(gvd_command_length, 2))
buf += bytes(int_to_bytes(magic_number1, 1))
buf += bytes(int_to_bytes(magic_number2, 1))
buf += bytes(int_to_bytes(op_code))
buf += bytes(int_to_bytes(param1))
buf += bytes(int_to_bytes(param2))
buf += bytes(int_to_bytes(param3))
buf += bytes(int_to_bytes(param4))
buf += bytearray(data)
l = list(buf)
res = debug_device.send_and_receive_raw_data(buf)
if res[0] == op_code:
res1 = res[4:]
return res1
else:
raise Exception("send_command return error", res[0])
except:
if i < retries - 1:
time.sleep(0.1)
else:
raise
def wait_for_rs_device(serial_no):
ctx = rs.context()
start = int(round(time.time() * 1000))
now = int(round(time.time() * 1000))
while now - start < 5000:
devices = ctx.query_devices()
for dev in devices:
pid = str(dev.get_info(rs.camera_info.product_id))
if len(serial_no) == 0 or serial_no == dev.get_info(rs.camera_info.serial_number):
print(' Device PID: ', dev.get_info(rs.camera_info.product_id))
print(' Device name: ', dev.get_info(rs.camera_info.name))
print(' Serial number: ', dev.get_info(rs.camera_info.serial_number))
print(' Product Line: ', dev.get_info(rs.camera_info.product_line))
print(' Firmware version: ', dev.get_info(rs.camera_info.firmware_version))
return dev
time.sleep(5)
now = int(round(time.time() * 1000))
raise Exception('No RealSense device' + str('.' if len(serial_no) == 0 else ' with serial number: ' + serial_no))
def main():
if any([help_str in sys.argv for help_str in ['-h', '--help', '/?']]):
print("Usage:", sys.argv[0], "[Options]")
print
print('[Options]:')
print('-i : /path/to/accel.txt [/path/to/gyro.txt]')
print('-s : serial number of device to calibrate.')
print('-g : show graph of norm values - original values in blue and corrected in green.')
print
print('If -i option is given, calibration is done using previosly saved files')
print('Otherwise, an interactive process is followed.')
sys.exit(1)
try:
accel_file = None
gyro_file = None
serial_no = ''
show_graph = '-g' in sys.argv
for idx in range(len(sys.argv)):
if sys.argv[idx] == '-i':
accel_file = sys.argv[idx + 1]
if len(sys.argv) > idx + 2 and not sys.argv[idx + 2].startswith('-'):
gyro_file = sys.argv[idx + 2]
if sys.argv[idx] == '-s':
serial_no = sys.argv[idx + 1]
print('waiting for realsense device...')
dev = wait_for_rs_device(serial_no)
product_line = dev.get_info(rs.camera_info.product_line)
if product_line == 'L500':
print('checking minimum firmware requirement ...')
fw_version = dev.get_info(rs.camera_info.firmware_version)
if fw_version < L515_FW_VER_REQUIRED:
raise Exception(
'L515 requires firmware ' + L515_FW_VER_REQUIRED + " or later to support IMU calibration. Please upgrade firmware and try again.")
else:
print(' firmware ' + fw_version + ' passed check.')
buckets = [[0, -g, 0], [g, 0, 0],
[0, g, 0], [-g, 0, 0],
[0, 0, -g], [0, 0, g]]
buckets_labels = ["Mounting screw pointing down, device facing out",
"Mounting screw pointing left, device facing out",
"Mounting screw pointing up, device facing out",
"Mounting screw pointing right, device facing out", "Viewing direction facing down",
"Viewing direction facing up"]
gyro_bais = np.zeros(3, np.float32)
old_settings = None
if accel_file:
if gyro_file:
gyro = np.loadtxt(gyro_file, delimiter=",")
gyro = gyro[gyro[:, 0] < gyro[0, 0] + 4000, :]
gyro_bais = np.mean(gyro[:, 1:], axis=0)
print(gyro_bais)
max_norm = np.linalg.norm(np.array([0.5, 0.5, 0.5]))
measurements = [[], [], [], [], [], []]
import csv
with open(accel_file, 'r') as csvfile:
reader = csv.reader(csvfile)
rnum = 0
for row in reader:
M = np.array([float(row[1]), float(row[2]), float(row[3])])
is_ok = False
for i in range(0, len(buckets)):
if np.linalg.norm(M - buckets[i]) < max_norm:
is_ok = True
measurements[i].append(M)
rnum += 1
print('read %d rows.' % rnum)
else:
print('Start interactive mode:')
if os.name == 'posix':
old_settings = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin.fileno())
imu = imu_wrapper()
if not imu.enable_imu_device(serial_no):
print('Failed to enable device.')
return -1
measurements, gyro = imu.get_measurements(buckets, buckets_labels)
con_mm = np.concatenate(measurements)
if os.name == 'posix':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
header = input(
'\nWould you like to save the raw data? Enter footer for saving files (accel_<footer>.txt and gyro_<footer>.txt)\nEnter nothing to not save raw data to disk. >')
print('\n')
if header:
accel_file = 'accel_%s.txt' % header
gyro_file = 'gyro_%s.txt' % header
print('Writing files:\n%s\n%s' % (accel_file, gyro_file))
np.savetxt(accel_file, con_mm, delimiter=',', fmt='%s')
np.savetxt(gyro_file, gyro, delimiter=',', fmt='%s')
else:
print('Not writing to files.')
measurements = [mm[:, 1:] for mm in measurements]
gyro_bais = np.mean(gyro[:, 1:], axis=0)
print(gyro_bais)
mlen = np.array([len(meas) for meas in measurements])
print(mlen)
print('using %d measurements.' % mlen.sum())
nrows = mlen.sum()
w = np.zeros([nrows, 4])
Y = np.zeros([nrows, 3])
row = 0
for i in range(0, len(buckets)):
for m in measurements[i]:
w[row, 0] = m[0]
w[row, 1] = m[1]
w[row, 2] = m[2]
w[row, 3] = -1
Y[row, 0] = buckets[i][0]
Y[row, 1] = buckets[i][1]
Y[row, 2] = buckets[i][2]
row += 1
np_version = [int(x) for x in np.version.version.split('.')]
rcond_val = None if (np_version[1] >= 14 or np_version[0] > 1) else -1
X, residuals, rank, singular = np.linalg.lstsq(w, Y, rcond=rcond_val)
print(X)
print("residuals:", residuals)
print("rank:", rank)
print("singular:", singular)
check_X(X, w[:, :3], show_graph)
calibration = {}
if product_line == 'L500':
calibration["device_type"] = "L515"
else:
calibration["device_type"] = "D435i"
calibration["imus"] = list()
calibration["imus"].append({})
calibration["imus"][0]["accelerometer"] = {}
calibration["imus"][0]["accelerometer"]["scale_and_alignment"] = X.flatten()[:9].tolist()
calibration["imus"][0]["accelerometer"]["bias"] = X.flatten()[9:].tolist()
calibration["imus"][0]["gyroscope"] = {}
calibration["imus"][0]["gyroscope"]["scale_and_alignment"] = np.eye(3).flatten().tolist()
calibration["imus"][0]["gyroscope"]["bias"] = gyro_bais.tolist()
json_data = json.dumps(calibration, indent=4, sort_keys=True)
directory = os.path.dirname(accel_file) if accel_file else '.'
with open(os.path.join(directory, "calibration.json"), 'w') as outfile:
outfile.write(json_data)
intrinsic_buffer = np.zeros([6, 4])
intrinsic_buffer[:3, :4] = X.T
intrinsic_buffer[3:, :3] = np.eye(3)
intrinsic_buffer[3:, 3] = gyro_bais
imu_calib_table = get_IMU_Calib_Table(intrinsic_buffer, product_line)
with open(os.path.join(directory, "calibration.bin"), 'wb') as outfile:
outfile.write(imu_calib_table.astype('f').tostring())
is_write = input('Would you like to write the results to the camera? (Y/N)')
is_write = 'Y' in is_write.upper()
if is_write:
print('Writing calibration to device.')
if product_line == 'L500':
l500_send_command(dev, WRITE_TABLE, 0, 0, 0, 0, imu_calib_table)
else:
calibration_table = get_calibration_table(imu_calib_table)
eeprom = get_eeprom(calibration_table)
write_eeprom_to_camera(eeprom, serial_no)
print('Done.')
else:
print('Abort writing to device')
except Exception as e:
print('\nError: %s' % e)
finally:
if os.name == 'posix' and old_settings is not None:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
"""
wtw = dot(transpose(w),w)
wtwi = np.linalg.inv(wtw)
print(wtwi)
X = dot(wtwi, Y)
print(X)
"""
if __name__ == '__main__':
main()
校准装置与Python校准脚本
-
过程概述
使用Python校准脚本校准设备的一般过程启动脚本以捕获6个不同位置的IMU数据,然后计算参数并将结果写入相机。在执行校准过程之前,阅读整个章节非常重要。
记录数据---------->计算校准---------->将校准写入设备
-
将设备连接到计算机
使用USB线将设备连接到已安装Intel®RealSense™SDK 2.0的PC上。
-
运行rs-imu-calibration.py文件
-
启动进程
-
在Ubuntu中使用bash终端或在Windows中使用命令提示符找到rs-imu-calibration.py的安装位置。
在命令提示符中:python rs-imu-calibration.py
-
脚本开始时的示例输出:
waiting for realsense device…
Device PID: 0B64
Device name: Intel RealSense L515
Serial number: 00000000f9340895
Product Line: L500 Firmware version: 01.04.01.00
checking minimum firmware requirement …
firmware 01.04.01.00 passed check.
Start interactive mode:
FOUND GYRO with fps=100 FOUND ACCEL with fps=100
***Press ESC to Quit ***
-
注意:在L515上,脚本将检查FW以确保它满足最低要求的FW 1.4.1.0。如果没有,它会停止并打印出一条警告消息,要求用户升级固件。
-
从6个位置捕获IMU数据
-
校准Python脚本中的校准算法需要设备的6个不同位置来计算校准。设备应该在每一个位置保持3到4秒。一定要在每个位置保持相机尽可能稳定。
-
由于产品设计和IMU在产品中的物理配置,D435i, D455和L515之间的位置不同,但在每个位置的目标是使IMU的轴与重力方向对齐,并按以下顺序描述。这些产品在设备的底部都有¼-20螺纹的三脚架安装螺纹,所以下面的校准脚本和说明将其作为设备物理定位的辅助。
-
使用真实的照片来更好地说明准确的位置和方向。
-
D435i and D455
-
计算校准
- 捕获所有6个位置后,脚本将提供一个提示,询问是否要保存原始数据。
-
将结果更新到设备
- 计算校准后,脚本提供了将结果写入相机的eeprom的选项。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)