Intel realsense d435i カメラ キャリブレーションの中国語ドキュメント
-
このドキュメントは英語の公式ドキュメントを指します。元のアドレスは Intel® RealSense™ Depth Camera 用 IMU Calibration Tool (intelrealsense.com) です。
-
IMU の概要: 慣性測定ユニット (imu) は、通常、システム インターナショナル (SI) 単位のメートル/秒で 2 乗 (m/s^2) を出力する加速度計と、通常、角速度を測定するジャイロスコープで構成されます。 SI 単位、ラジアン/秒 (rad/s)。Intel RealSense™ Depth Camera D435i および D455 カメラと Intel® RealSense™ LiDAR Camera L515 の IMU も同様で、出力周波数を構成できる加速度計とジャイロスコープが含まれています。
-
IMU キャリブレーション パラメータ: IMU キャリブレーション パラメータには、内部パラメータと外部パラメータが含まれます。考えられる IMU キャリブレーション パラメータは多数ありますが、簡単にするために次のパラメータを考慮してみましょう。
-
加速度センサーの場合:
-
スケール係数 (感度) – これは、出力がメートルスケールになるように元の測定値を乗算するために使用される用語です。数学的には次のように書かれます
-
バイアス (ゼロ オフセット) - センサーがゼロを読み取る必要がある場合に、ゼロ以外の値を除去するために使用される用語。数学的には次のように書かれます
-
軸外項 - これらの項は、加速度計の軸が直交していない場合に補正するために使用されます。数学的には次のように書かれます
-
-
ジャイロスコープの場合:
-
バイアス (ゼロ オフセット) - センサーがゼロを読み取る必要がある場合に、ゼロ以外の値をキャンセルするために使用される用語。数学的には次のように書かれます
-
固有パラメータは、センサーの不正確さをモデル化することにより、生のセンサー データを実際の測定値に変換しようとします。加速度計の数学的変換は次のとおりです。
この場合、ジャイロスコープは次の単純なモデルに従います。
IMU ininic パラメータの基本的な知識については、STMicroelectronics: PDF アドレスを参照してください。
外部パラメータには次のものが含まれます。
- 回転 - 左赤外線 (IR) カメラ (IR1) から IMU までの回転。3 × 3 回転行列として指定します。
- 移動 – 左 IR カメラから IMU までの移動量。ミリメートル単位の 3 × 1 ベクトルとして指定します。
この文書の範囲は、内部パラメータ校正プロセスのみをカバーします。外部パラメータは libaresense で使用できます。
IMUキャリブレーションツール
キャリブレーション Python スクリプト rs-imu-calibration.py は、インテル® RealSense™ SDK 2.0 に含まれています。詳細については、このドキュメントの後半を参照してください。D435i、D455、L515 カメラを例として、IMU キャリブレーション プロセスを紹介します。
制限と精度
提供されているツールでは全体的に良好なキャリブレーション結果が得られますが、すべての手順に従わない場合、キャリブレーションは正確ではありません。次の要因も補正効果に影響を与える可能性があります。
-
キャリブレーションでは、指定された 6 つの方向 (向きに応じて水平または垂直) にカメラ デバイスを位置決めして調整する必要があります。アライメントを改善すると、エラーが最小限に抑えられ、精度が向上します。重力に最も近づけるために 3 軸レベリングを使用することをお勧めします。
-
このツールはすべての方向で最適化され、平均加速度の結果は重力による加速度 (9.8 m/s^2) に近くなりますが、場合によっては、単一方向の加速度が重力による完全な加速度よりわずかに小さいか超える場合があります。 。
ユーザー定義のキャリブレーション
ユーザーが独自のツールを使用して特定のアプリケーションのニーズを満たすようにデバイスを調整したい場合は、rs-imu-calibration.py で示されているように結果をデバイスに書き込むことができます。
IMU 校正テーブルの形式については、PDF フォームを参照してください。
テーブルの形式は、テーブル ヘッダーのバージョンとテーブル タイプのフィールドを除き、すべてのデバイスで同じです。
[外部リンク画像の転送に失敗しました。ソース サイトにはリーチ防止メカニズムがある可能性があります。画像を保存して直接アップロードすることをお勧めします (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 キャリブレーションをサポートする予定です。
インストール
このセクションでは、Calibrate Python スクリプトを実行してデバイスを調整するために必要なハードウェアとソフトウェアのセットアップについて説明します。
ハードウェア
必要なハードウェアには、キャリブレーション対象の D435i、D455、または L515 デバイス、USB ケーブル、Windows 10、Ubuntu 16.04、および Ubuntu 18.04 を実行しているコンピューターが含まれます。
-
装置
- キャリブレーション プロセスは、以下に示すインテル® RealSense™ デプス カメラ D435i、インテル® RealSense™ デプス カメラ D455、またはインテル® RealSense™ LiDAR カメラ L515 デバイスを使用して示されています。
- キャリブレーション プロセスは、以下に示すインテル® RealSense™ デプス カメラ D435i、インテル® RealSense™ デプス カメラ D455、またはインテル® RealSense™ LiDAR カメラ L515 デバイスを使用して示されています。
-
USB
- USB-typeC データケーブルは、デバイスとホストの接続に使用されます。
-
パソコン
- Windows 10 または Ubuntu
ソフトウェア: 次のソフトウェアをホストにインストールします。
- Windows 上の Python: Python をダウンロードしてインストールします: https://www.python.org/downloads/windows/
- Python キャリブレーション スクリプト: キャリブレーション スクリプト (インテル® RealSense™ SDK の一部) rs-imu-calibration.py、https://github.com/IntelRealSense/librealsense で入手できます。すべてのキャリブレーション機能がサポートされていることを確認するには、最新バージョンをダウンロードしてください。このファイルは、ソース ツリーの 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 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 # SI Gravity page 52 of https://nvlpubs.nist.gov/nistpubs/Legacy/SP/nistspecialpublication330e2008.pdf
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) # 0 - idle, 1 - rotate to position, 2 - wait to stable, 3 - pick data
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': # x1b is ESC
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
## Status.collect_data
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)
# sys.stdout.write(': %-3.1f (secs)' % (self.time_to_collect - elapsed_time))
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 elapsed_time > self.time_to_collect:
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
## Status.rotate
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
## Status.wait_to_stable
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
# self.sync_imu_by_this_stream = rs.stream.any
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)
# 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 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) # convert from signed to unsigned 32 bit
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] # 520 == epprom++
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] # calibration table
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] # D435 IMU Calib Table
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] # calibration data
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 # according to table "D435 IMU Calib Table" here: https://user-images.githubusercontent.com/6958867/50902974-20507500-1425-11e9-8ca5-8bd2ac2d0ea1.png
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 # according to table "D435 IMU Calib Table" in "https://sharepoint.ger.ith.intel.com/sites/3D_project/Shared%20Documents/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) * 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
# data_size = calibration_table.size
header_size = header.size()
size_of_buffer = DC_MM_EEPROM_SIZE
data_size = size_of_buffer - 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
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 = 520
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 # command 1 = 0x50
# command 2 = 0
# command 3 = 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
# 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 = 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(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 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.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 = 0xab
magic_number2 = 0xcd
buf = 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 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(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 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]]
# 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 = None
if accel_file:
if gyro_file:
# compute gyro bais
# assume the first 4 seconds the device is still
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)
# compute accel intrinsic parameters
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.')
# 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 = 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)
# concatinate the two 12 element arrays and save
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
# 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()
-
インテル® RealSense™ SDK 2.0: 最新バージョンのインテル® RealSense™ SDK をインストールします。表 3-1 には、SDK ホームページ、GitHub* リポジトリ (最新バージョンをダウンロードできる場所)、および SDK ドキュメントへのポインタが含まれています。
-
ピップ / ナンピー / 列挙型
-
pyrealsense2 (Python ライブラリ)
キャリブレーションデバイスとPythonキャリブレーションスクリプト
-
プロセスの概要
Python キャリブレーション スクリプトを使用してデバイスをキャリブレーションする一般的なプロセスでは、スクリプトを開始して 6 つの異なる場所で IMU データをキャプチャし、パラメータを計算して結果をカメラに書き込みます。校正手順を実行する前に、この章全体を読むことが重要です。
データの記録----------> キャリブレーションの計算----------> デバイスへのキャリブレーションの書き込み
-
デバイスをコンピュータに接続します
USB ケーブルを使用して、インテル® RealSense™ SDK 2.0 がインストールされた PC にデバイスを接続します。
-
rs-imu-calibration.py ファイルを実行します。
-
開始プロセス
-
Ubuntu の bash ターミナルまたは Windows のコマンド プロンプトを使用して、rs-imu-calibration.py がインストールされている場所を見つけます。
コマンド プロンプトで: python rs-imu-calibration.py -
スクリプトの先頭の出力例:
Realsense デバイスを待っています…
デバイスPID: 0B64
デバイス名: Intel RealSense L515
シリアル番号: 00000000f9340895
製品ライン: L500 ファームウェアバージョン: 01.04.01.00
ファームウェアの最小要件を確認中 …
ファームウェア 01.04.01.00 のチェックに合格しました。
インタラクティブモードを開始します。
ジャイロを検出 (fps=100) アクセルを検出 (fps=100)
***ESC を押して終了します ***
-
注: L515 では、スクリプトは FW をチェックして、FW 1.4.1.0 の最小要件を満たしていることを確認します。そうでない場合、プログラムは停止し、ユーザーにファームウェアをアップグレードするよう求める警告メッセージを出力します。
-
-
-
6 か所から IMU データをキャプチャ
-
キャリブレーション Python スクリプトのキャリブレーション アルゴリズムでは、キャリブレーションを計算するためにデバイスの 6 つの異なる位置が必要です。デバイスは各位置に 3 ～ 4 秒間保持する必要があります。各位置でカメラをできるだけ安定させてください。
-
製品設計と製品内の IMU の物理構成により、D435i、D455、L515 では位置が異なりますが、各位置の目標は、以下で説明するように、IMU の軸を重力の方向に揃えることです。注文。これらの製品はすべて、デバイスの底部に 1/4-20 ネジの三脚取り付けネジを備えているため、以下のキャリブレーション スクリプトと手順では、これをデバイスの物理的な位置決めの補助として使用します。
-
正確な場所と方向をよりわかりやすく説明するには、実際の写真を使用します。
-
D435i と D455
-
データは、キャリブレーション スクリプトの指示に従って、次の場所で D435i デバイスを使用してキャプチャされました。D455 の物理的な形状と USB ポートの位置は若干異なりますが、全体的なキャリブレーション手順は似ています。
-
位置 #1 - 取り付けネジは下向き、デバイスは外側を向いています
-
録画を開始したら、前述したようにカメラをこの位置に 3 ～ 4 秒間静止させてください。スクリプトは、各場所に関するヒントとガイダンスを提供します。
-
出力例
-
-
位置 2 - 取り付けネジを左に、デバイスを外側に向けます。カメラを前のセクションで説明したのと同じ方向に向けた状態で、カメラの視線方向を中心にカメラを 90 度回転させ、1/4-20 ネジ付き三脚が左を向くようにします。
- カメラをこの位置に 3 ～ 4 秒間静止させます。
- カメラをこの位置に 3 ～ 4 秒間静止させます。
-
位置 3 - 取り付けネジは上向き、デバイスは外側を向いています
-
カメラの視線方向に対して位置 2 からカメラをさらに 90 度回転させ、カメラが上下逆になり、1/4-20 ネジ付き三脚マウントに上を向くようにします。
-
カメラをこの位置に 3 ～ 4 秒間静止させます。
-
-
位置 4 - 取り付けネジは右を向き、デバイスは外側を向いています
-
カメラを位置 3 からさらに 90 度回転させ、1/4-20 ネジ付き三脚マウントが右を向くようにします。
-
カメラをこの位置に 3 ～ 4 秒間静止させます。
-
-
姿勢 5 - 視線方向を下に向ける
-
Intel® RealSense™ ロゴが上を向くように、カメラの表示方向を下に向けます。
-
カメラをこの位置に 3 ～ 4 秒間静止させます。
-
-
姿勢6 - 視線を上に向ける
-
Intel® RealSense™ ロゴが下を向くように、位置 5 から USB ケーブルを中心にカメラを 180 度回転します。
-
カメラをこの位置に 3 ～ 4 秒間静止させます。録画が停止するまで、カメラをこの最終位置に置いておきます。
-
-
-
-
校正を計算する
- 6 つの場所すべてがキャプチャされると、スクリプトは生データを保存するかどうかを尋ねるプロンプトを表示します。
- 6 つの場所すべてがキャプチャされると、スクリプトは生データを保存するかどうかを尋ねるプロンプトを表示します。
-
結果をデバイスに更新する
- キャリブレーションを計算した後、スクリプトは結果をカメラの EEPROM に書き込むオプションを提供します。