保存地图
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))
sensor = device.sensors[0]
sensor.set_notifications_callback(cb)
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:
data = pose.get_pose_data()
if a !=0 and data.tracker_confidence>2:
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()
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))
sensor = device.sensors[0]
sensor.set_notifications_callback(cb)
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)
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)
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
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)