英特尔 t265 保存地图 (Python API)

保存地图

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