保存地图
import pyrealsense2 as rs
import time
import threading
a = 0
def recode():
global a
while True:
a = input("please input a:")
print(type(a))
a = int(a)
if a==9:
break
def cb(notification):
print(notification)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.pose)
device = config.resolve(pipeline).get_device()
print("Firmware:", device.get_info(rs.camera_info.firmware_version))
print("Serial Number:", device.get_info(rs.camera_info.serial_number))
print("USB:", device.get_info(rs.camera_info.usb_type_descriptor))
# Get first pyrealsense2.sensor in order to set notification callback
sensor = device.sensors[0]
sensor.set_notifications_callback(cb)
# Get first pyrealsense2.pose_sensor in order to set options, load maps, save maps, set static node, get static node
pose_sensor = device.first_pose_sensor()
# 此选项将允许设备在停止/开始调用时保留其当前映射。设备将表现得好像地图在停止后保存,并在后续启动之前加载回去。
pose_sensor.set_option(rs.option.enable_map_preservation, 1)
pipeline.start(config)
t1 = threading.Thread(target=recode)
t1.start()
try:
while True:
frames = pipeline.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
# Print some of the pose data to the terminal
data = pose.get_pose_data()
# Pose confidence 0x0 - Failed, 0x1 - Low, 0x2 - Medium, 0x3 - High
# 当追踪置信度高且a不等于0的情况下写入静态点
if a !=0 and data.tracker_confidence>2:
# 输入9退出
if a==9:
break
data = pose.get_pose_data()
node_name = "test" + str(a)
pose_sensor.set_static_node(node_name,data.translation,data.rotation)
print("set node " + node_name + " success!")
print(data.translation)
a = 0
finally:
pipeline.stop()
a = 9
pass
time.sleep(5)
slam_map = pose_sensor.export_localization_map() # Fails here
print(slam_map)
# 保存当前地图
with open("Local.map", 'wb') as fp:
for i in range(len(slam_map)):
fp.write(slam_map[i].to_bytes(1, byteorder='little'))
读取地图
import pyrealsense2 as rs
import time
def cb(notification):
print(notification)
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.pose)
device = config.resolve(pipeline).get_device()
print("Firmware:", device.get_info(rs.camera_info.firmware_version))
print("Serial Number:", device.get_info(rs.camera_info.serial_number))
print("USB:", device.get_info(rs.camera_info.usb_type_descriptor))
# Get first pyrealsense2.sensor in order to set notification callback
sensor = device.sensors[0]
sensor.set_notifications_callback(cb)
# Get first pyrealsense2.pose_sensor in order to set options, load maps, save maps, set static node, get static node
pose_sensor = device.first_pose_sensor()
pose_sensor.set_option(rs.option.enable_map_preservation, 1)
var = []
with open("Local.map",'rb') as fp:
while(True):
data = fp.read(1) # type(data) === bytes
if data == b'':
break
text = int.from_bytes(data, byteorder='little')
var.append(text)
print("loading")
if pose_sensor.import_localization_map(var):
print("load map!!!")
time.sleep(5)
else:
print("false")
pipeline.start(config)
try:
while True:
frames = pipeline.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
print(data.tracker_confidence)
# 获得静态点消息
for i in range(0,10):
node_name = "test" + str(i)
# 只有当追踪置信度高度3时该API才能调用成功
result, translation, rotation = pose_sensor.get_static_node(node_name)
if result:
print("get node " + node_name)
print(translation)
finally:
pipeline.stop()
注:T265保存的地图数据是不透明的。与地图交互的唯一方式是在保存之前,使用set_static_node相对于当前会话坐标的命名位置
C++版:librealsense/examples/ar-advanced at master · IntelRealSense/librealsense · GitHub