intel realsense d435i相机标定中文文档

编程入门 行业动态 更新时间:2024-10-07 10:21:54

intel realsense d435i相机标定<a href=https://www.elefans.com/category/jswz/34/1769975.html style=中文文档"/>

intel realsense d435i相机标定中文文档

intel realsense d435i相机标定中文文档

  • 此文档参考了官方的英文文档,原地址面向英特尔®实感™深度摄像头的 IMU 校准工具 (intelrealsense)

  • 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的标定过程。

局限性和准确性

虽然所提供的工具取得了良好的整体校准结果,但如果不遵循所有步骤,则校准将不准确。下列因素亦可能影响校正效果:

  • 校准需要将相机设备定位在6个指定的方向和对齐(水平或垂直取决于方向)。更好的对齐最小化误差并提高精度。建议使用3轴水平,以确保最接近重力。

  • 该工具优化了所有方向,虽然平均加速度结果接近重力加速度(9.8 m/s^2),但有时单个方向的加速度可能略低于或超过完美重力加速度。

用户自定义校准

如果用户更喜欢使用自己的工具校准设备以满足特定的应用需求,结果可以按照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 要求

  • 支持IMU校准需要Intel®RealSense™LiDAR Camera L515 FW 1.4.1.0及以上版本。

  • Intel®RealSense™深度相机D435i/D455 FW随相机发布,稍后支持IMU校准。

安装

本节介绍运行校准Python脚本校准设备所需的硬件和软件设置。

硬件

所需硬件包括待校准的D435i、D455或L515设备、一根USB线缆、一台运行Windows 10、Ubuntu 16.04和Ubuntu 18.04的计算机。

  • 设备

    • 使用如下所示的Intel®RealSense™深度相机D435i, Intel®RealSense™深度相机D455,或Intel®RealSense™LiDAR相机L515设备来显示校准过程。
  • USB

    • USB-typeC数据线,用于连接设备和主机。
  • PC

    • Windows 10 or ubuntu

软件:在主机上安装以下软件

  • Python On Windows: Download and install Python: /
  • Python校准脚本:校准脚本(Intel®RealSense™SDK的一部分)rs-imu-calibration.py,可在。请下载最新版本,以确保支持所有校准功能。该文件位于源代码树的tools/rs-imu-calibration目录中。
#rs-imu-calibration.py
#!/usr/bin/python
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# L515
READ_TABLE = 0x43  # READ_TABLE 0x243 0
WRITE_TABLE = 0x44  # WRITE_TABLE 0 <table># L515 minimum firmware version required to support IMU calibration
L515_FW_VER_REQUIRED = '01.04.01.00'is_data = None
get_key = None
if os.name == 'posix':import selectimport ttyimport termiosis_data = lambda: select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])get_key = lambda: sys.stdin.read(1)elif os.name == 'nt':import msvcrtis_data = msvcrt.kbhitget_key = lambda: msvcrt.getch()else:raise Exception('Unsupported OS: %s' % os.name)if sys.version_info[0] < 3:input = raw_inputmax_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  # SI Gravity page 52 of .pdfCOLOR_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 & 0xffnum >>= 8if num:raise OverflowError("Number {} doesn't fit into {} bytes.".format(num, length))if order == 'little':res.reverse()return resdef 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] & 0xffffffffelse:return struct.unpack('>i', struct.pack('BBBB', *([0] * (4 - len(bytes_array))) + bytes_array))[0] & 0xffffffffclass imu_wrapper:class Status(enum.Enum):idle = 0,rotate = 1,wait_to_stable = 2,collect_data = 3def __init__(self):self.pipeline = Noneself.imu_sensor = Noneself.status = self.Status(self.Status.idle)  # 0 - idle, 1 - rotate to position, 2 - wait to stable, 3 - pick dataself.thread = threading.Condition()self.step_start_time = time.time()self.time_to_stable = 3self.time_to_collect = 2self.samples_to_collect = 1000self.rotating_threshold = 0.1self.moving_threshold_factor = 0.1self.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 = 20self.is_done = Falseself.is_data = Falsedef escape_handler(self):self.thread.acquire()self.status = self.Status.idleself.is_done = Trueself.thread.notify()self.thread.release()sys.exit(-1)def imu_callback(self, frame):if not self.is_data:self.is_data = Truewith self.callback_lock:try:if is_data():c = get_key()if c == '\x1b':  # x1b is ESCself.escape_handler()if self.status == self.Status.idle:returnpr = 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## Status.collect_dataif 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)# sys.stdout.write(': %-3.1f (secs)' % (self.time_to_collect - elapsed_time))color = COLOR_GREENif 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_normif is_in_norm:self.collected_data_accel.append(np.append(frame.get_timestamp(), data_np))else:color = COLOR_REDis_moving = abs(np.linalg.norm(data_np) - g) / g > self.moving_threshold_factorsys.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.rotatereturn# if elapsed_time > self.time_to_collect:if part_done >= 1:self.status = self.Status.collect_datasys.stdout.write('\n\nDirection data collected.')self.thread.acquire()self.status = self.Status.idleself.thread.notify()self.thread.release()returnif pr.stream_type() == rs.stream.gyro:returnsys.stdout.write('\r %15s' % self.status)crnt_dir = np.array(data_np) / np.linalg.norm(data_np)crnt_diff = self.crnt_direction - crnt_diris_in_norm = np.linalg.norm(data_np - self.crnt_bucket) < self.max_norm## Status.rotateif 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_stablesys.stdout.write('\r' + ' ' * 90)self.step_start_time = time.time()return## Status.wait_to_stableif 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.rotatereturnif elapsed_time > self.time_to_stable:self.collected_data_gyro = []self.collected_data_accel = []self.status = self.Status.collect_dataself.step_start_time = time.time()returnreturnexcept Exception as e:print('ERROR?' + str(e))self.thread.acquire()self.status = self.Status.idleself.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.rotateself.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# self.sync_imu_by_this_stream = rs.stream.anyactive_imu_profiles = []active_profiles = dict()self.imu_sensor = Nonefor 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()] = prself.imu_sensor = sensorif pr.stream_type() == rs.stream.accel and pr.format() == rs.format.motion_xyz32f:active_profiles[pr.stream_type()] = prself.imu_sensor = sensorif self.imu_sensor:breakif not self.imu_sensor:print('No IMU sensor found.')return Falseprint('\n'.join(['FOUND %s with fps=%s' % (str(ap[0]).split('.')[1].upper(), ap[1].fps()) for ap inactive_profiles.items()]))active_imu_profiles = list(active_profiles.values())if len(active_imu_profiles) < 2:print('Not all IMU streams found.')return Falseself.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)# Make the device use the original IMU values and not already calibrated:if self.imu_sensor.supports(rs.option.enable_motion_correction):self.imu_sensor.set_option(rs.option.enable_motion_correction, 0)return Trueclass CHeader:def __init__(self, version, table_type):self.buffer = np.ones(16, dtype=np.uint8) * 255self.buffer[0] = int(version[0], 16)self.buffer[1] = int(version[1], 16)self.buffer.dtype = np.uint16self.buffer[1] = int(table_type, 16)def size(self):return 16def set_data_size(self, size):self.buffer.dtype = np.uint32self.buffer[1] = sizedef set_crc32(self, crc32):self.buffer.dtype = np.uint32self.buffer[3] = crc32 % (1 << 32)  # convert from signed to unsigned 32 bitdef get_buffer(self):self.buffer.dtype = np.uint8return self.bufferdef 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 = 24header_size = 16buffer.dtype = np.uint32tab1_size = buffer[3]buffer.dtype = np.uint8print('tab1_size (all_data): ', tab1_size)tab1 = buffer[cmd_size:cmd_size + tab1_size]  # 520 == epprom++tab1.dtype = np.uint32tab2_size = tab1[1]tab1.dtype = np.uint8print('tab2_size (calibration_table): ', tab2_size)tab2 = tab1[header_size:header_size + tab2_size]  # calibration tabletab2.dtype = np.uint32tab3_size = tab2[1]tab2.dtype = np.uint8print('tab3_size (calibration_table): ', tab3_size)tab3 = tab2[header_size:header_size + tab3_size]  # D435 IMU Calib Tabletab3.dtype = np.uint32tab4_size = tab3[1]tab3.dtype = np.uint8print('tab4_size (D435_IMU_Calib_Table): ', tab4_size)tab4 = tab3[header_size:header_size + tab4_size]  # calibration datareturn tab1, tab2, tab3, tab4def 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 + 96size_of_buffer = header_size + data_size  # according to table "D435 IMU Calib Table" here: .pngassert (size_of_buffer % 4 == 0)buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255use_extrinsics = Falseuse_intrinsics = Truedata_buffer = np.ones(data_size, dtype=np.uint8) * 255data_buffer.dtype = np.float32data_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_vectordata_buffer.dtype = np.uint8header.set_data_size(data_size)header.set_crc32(binascii.crc32(data_buffer))buffer[:header_size] = header.get_buffer()buffer[header_size:] = data_bufferreturn bufferdef 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.sizesn_table_size = 32data_size = d435_imu_calib_table_size + sn_table_sizeheader_size = header.size()size_of_buffer = header_size + data_size  # according to table "D435 IMU Calib Table" in "/Arch/D400/FW/D435i_IMU_Calibration_eeprom_0_52.xlsx"assert (size_of_buffer % 4 == 0)buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255data_buffer = np.ones(data_size, dtype=np.uint8) * 255data_buffer[:d435_imu_calib_table_size] = d435_imu_calib_tableheader.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_bufferreturn bufferdef get_eeprom(calibration_table):version = ['0x01', '0x01']table_type = '0x09'header = CHeader(version, table_type)DC_MM_EEPROM_SIZE = 520# data_size = calibration_table.sizeheader_size = header.size()size_of_buffer = DC_MM_EEPROM_SIZEdata_size = size_of_buffer - header_size# size_of_buffer = header_size + data_sizeassert (size_of_buffer % 4 == 0)buffer = np.ones(size_of_buffer, dtype=np.uint8) * 255header.set_data_size(data_size)buffer[header_size:header_size + calibration_table.size] = calibration_tableheader.set_crc32(binascii.crc32(buffer[header_size:]))buffer[:header_size] = header.get_buffer()return bufferdef write_eeprom_to_camera(eeprom, serial_no=''):# DC_MM_EEPROM_SIZE = 520DC_MM_EEPROM_SIZE = eeprom.sizeDS5_CMD_LENGTH = 24MMEW_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) * 255cmd = np.array(struct.unpack('I' * 6, MMEW_Cmd_bytes), dtype=np.uint32)cmd.dtype = np.uint16cmd[0] += DC_MM_EEPROM_SIZEcmd.dtype = np.uint32cmd[3] = DC_MM_EEPROM_SIZE  # command 1 = 0x50# command 2 = 0# command 3 = sizecmd.dtype = np.uint8buffer[:len(cmd)] = cmdbuffer[len(cmd):len(cmd) + eeprom.size] = eepromdebug = get_debug_device(serial_no)if not debug:print('Error getting RealSense Device.')return# tab1, tab2, tab3, tab4 = parse_buffer(buffer)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 = Falsefor dev in devices:if len(serial_no) == 0 or serial_no == dev.get_info(rs.camera_info.serial_number):found_dev = Truebreakif not found_dev:print('No RealSense device found' + str('.' if len(serial_no) == 0 else ' with serial number: ' + serial_no))return 0# print(a few basic information about the device)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 debugdef 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 pylabpylab.plot(norm_data, '.b')# pylab.hold(True)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 = 0xabmagic_number2 = 0xcdbuf = bytearray()buf += bytes(int_to_bytes(gvd_command_length, 2))# buf += bytes(int_to_bytes(0, 1))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 res1else:raise Exception("send_command return error", res[0])except:if i < retries - 1:time.sleep(0.1)else:raisedef 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(a few basic information about the device)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 devtime.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]")printprint('[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.')printprint('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 = Nonegyro_file = Noneserial_no = ''show_graph = '-g' in sys.argvfor 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]]# all D400 and L500 cameras with IMU equipped with a mounting screw at the bottom of the device# when device is in normal use position upright facing out, mount screw is pointing down, aligned with positive Y direction in depth coordinate system# IMU output on each of these devices is transformed into the depth coordinate system, i.e.,# looking from back of the camera towards front, the positive x-axis points to the right, the positive y-axis points down, and the positive z-axis points forward.# output of motion data is consistent with convention that positive direction aligned with gravity leads to -1g and opposite direction leads to +1g, for example,# positive z_aixs points forward away from front glass of the device,#  1) if place the device flat on a table, facing up, positive z-axis points up, z-axis acceleration is around +1g#  2) facing down, positive z-axis points down, z-axis accleration would be around -1g#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 = Noneif accel_file:if gyro_file:# compute gyro bais# assume the first 4 seconds the device is stillgyro = np.loadtxt(gyro_file, delimiter=",")gyro = gyro[gyro[:, 0] < gyro[0, 0] + 4000, :]gyro_bais = np.mean(gyro[:, 1:], axis=0)print(gyro_bais)# compute accel intrinsic parametersmax_norm = np.linalg.norm(np.array([0.5, 0.5, 0.5]))measurements = [[], [], [], [], [], []]import csvwith open(accel_file, 'r') as csvfile:reader = csv.reader(csvfile)rnum = 0for row in reader:M = np.array([float(row[1]), float(row[2]), float(row[3])])is_ok = Falsefor i in range(0, len(buckets)):if np.linalg.norm(M - buckets[i]) < max_norm:is_ok = Truemeasurements[i].append(M)rnum += 1print('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 -1measurements, 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' % headergyro_file = 'gyro_%s.txt' % headerprint('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.')# remove times from measurements: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 = 0for 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] = -1Y[row, 0] = buckets[i][0]Y[row, 1] = buckets[i][1]Y[row, 2] = buckets[i][2]row += 1np_version = [int(x) for x in np.version.version.split('.')]rcond_val = None if (np_version[1] >= 14 or np_version[0] > 1) else -1X, 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)# concatinate the two 12 element arrays and saveintrinsic_buffer = np.zeros([6, 4])intrinsic_buffer[:3, :4] = X.Tintrinsic_buffer[3:, :3] = np.eye(3)intrinsic_buffer[3:, 3] = gyro_bais# intrinsic_buffer = ((np.array(range(24),np.float32)+1)/10).reshape([6,4])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()
  • Intel® RealSense™ SDK 2.0:安装最新版本的英特尔®RealSense™SDK。表3-1包含了指向SDK主页、GitHub*仓库(可以下载最新版本)和SDK文档的指针。

  • Pip / Numpy / Enum

  • pyrealsense2(python库)

校准装置与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

      • 根据校准脚本的指示,在以下位置用D435i设备捕获数据。D455的物理形状和USB端口位置略有不同,但总体校准程序相似。

      • 位置#1 -安装螺钉指向下方,设备朝外

        • 开始录制后,如上所述,保持相机在这个位置不动3到4秒。该脚本将为每个位置提供提示和指导。

        • 输出示例

      • 位置2 -安装螺钉向左,设备朝外:摄像机朝向上节所述的相同方向,围绕摄像机的观看方向旋转摄像机90度,使¼- 20螺纹三脚架指向左侧。

        • 保持相机在这个位置不动3到4秒。
      • 位置3 -安装螺钉指向上,设备朝向外

        • 从2号位置旋转相机一个额外的90度关于相机的观察方向,这样现在相机是倒置的¼- 20螺纹三脚架架面对上。

        • 保持相机在这个位置不动3到4秒。

      • 位置4 -安装螺钉指向右侧,设备朝外

        • 从3号位置旋转相机一个额外的90度,使¼- 20螺纹三脚架安装指向右边。

        • 保持相机在这个位置不动3到4秒。

      • 姿势5 -观察方向朝下

        • 将相机的观看方向朝下,使英特尔®RealSense™标志朝上。

        • 保持相机在这个位置不动3到4秒。

      • 姿势6 -观察方向朝上

        • 从5号位置将摄像头围绕USB线旋转180度,使英特尔®RealSense™标志朝下。

        • 保持相机在这个位置不动3到4秒。让相机保持在这个最后的位置,直到录音停止。

  • 计算校准

    • 捕获所有6个位置后,脚本将提供一个提示,询问是否要保存原始数据。
  • 将结果更新到设备

    • 计算校准后,脚本提供了将结果写入相机的eeprom的选项。

更多推荐

intel realsense d435i相机标定中文文档

本文发布于:2024-02-25 08:19:54,感谢您对本站的认可!
本文链接:https://www.elefans.com/category/jswz/34/1698347.html
版权声明:本站内容均来自互联网,仅供演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,我们将在24小时内删除。
本文标签:中文   相机   文档   intel   realsense

发布评论

评论列表 (有 0 条评论)
草根站长

>www.elefans.com

编程频道|电子爱好者 - 技术资讯及电子产品介绍!