realsense d435i获取 imu数据

 

 

import pyrealsense2.pyrealsense2 as rs
import configparser
import numpy as np
import time
import csv
import cv2

config_pro = configparser.ConfigParser()
config_pro.read('start.ini')
config_pro.sections()

D_cam = config_pro['D435i'].getboolean('Camera')
T_cam = config_pro['T265'].getboolean('Camera')

def Start_pipe():
    global p, cfg

    p = rs.pipeline(ctx) 
    cfg = rs.config()

def Read_dev():
    global dev

    dev_serial = dev.get_info(rs.camera_info.serial_number)
    del dev 
    cfg.enable_device(dev_serial)
    p.start(cfg)

#Start camera D435I
def d_start():
    global dev, T_cam, colorwriter, depthwriter, pipelines_d
    
    Start_pipe()

    D_dweidht = int(config_pro['D435i']['Resolution_DWidht'])
    D_dheight = int(config_pro['D435i']['Resolution_DHeight'])
    D_cweidht = int(config_pro['D435i']['Resolution_CWidht'])
    D_cheight = int(config_pro['D435i']['Resolution_CHeight'])
                
    cfg.enable_stream(rs.stream.accel,rs.format.motion_xyz32f,63)
    cfg.enable_stream(rs.stream.gyro,rs.format.motion_xyz32f,200)
                
    cfg.enable_stream(rs.stream.depth, D_dweidht, D_dheight, rs.format.z16, fps)
    cfg.enable_stream(rs.stream.color, D_cweidht, D_cheight, rs.format.bgr8, fps)    
                
    if video:
                
        color_path = 'color.avi'
        depth_path = 'depth.avi'

        colorwriter = cv2.VideoWriter(color_path, cv2.VideoWriter_fourcc(*'XVID'), fps, (D_cweidht, D_cheight), 1)
        depthwriter = cv2.VideoWriter(depth_path, cv2.VideoWriter_fourcc(*'XVID'), fps, (D_dweidht, D_dheight), 1)
    
    Read_dev()
    pipelines_d = p

#Start camera T265
def t_start():
    global dev, pipelines_t, D_cam, rightwriter, leftwriter
    
    Start_pipe() 

    T_weidht = int(config_pro['T265']['Resolution_Widht'])
    T_height = int(config_pro['T265']['Resolution_Height'])

    cfg.enable_stream(rs.stream.pose)
    cfg.enable_stream(rs.stream.gyro)
    cfg.enable_stream(rs.stream.fisheye, 1)
    cfg.enable_stream(rs.stream.fisheye, 2)

    if video:

        right_path = 'right.avi'
        left_path = 'left.avi'

        rightwriter = cv2.VideoWriter(right_path, cv2.VideoWriter_fourcc(*'XVID'), fps, (T_weidht, T_height), 1)
        leftwriter = cv2.VideoWriter(left_path, cv2.VideoWriter_fourcc(*'XVID'), fps, (T_weidht, T_height), 1)

    Read_dev()
    pipelines_t = p

#Check camera states and start accessible ones
if D_cam or T_cam:
    
    start_cam = True

    fps = int(config_pro['DEFAULT']['FPS'])
    video = config_pro['DEFAULT'].getboolean('Video')
    data_write = config_pro['DEFAULT'].getboolean('Data_Write')
    timer = float(config_pro['DEFAULT']['Time'])
    data_file = config_pro['DEFAULT'].getboolean('Data_File')

    #Append camera states to array
    ar_cam = []
    ctx = rs.context()
    for dev in ctx.query_devices():
        dev.hardware_reset()
        time.sleep(1)
        message = dev.get_info(rs.camera_info.name)
        print(message)
        d_bool = "D435I" in message
        t_bool = "T265" in message
        ar_cam.append(d_bool)
        ar_cam.append(t_bool)

    #Start accessible cameras and block others
    if len(ar_cam)>0:
        if len(ar_cam)>2:
            if (D_cam and T_cam) or not (D_cam and T_cam):
                counter=0
                for dev in ctx.query_devices():
                    counter=counter+1
                    if counter==1:
                        d_start()
                    elif counter==2:
                        t_start()
            else:
                if D_cam and ar_cam[0]:
                    d_start()
                    T_cam = False
                elif T_cam and ar_cam[2]:
                    t_start()
                    D_cam = False
        elif len(ar_cam)<3:
            if D_cam and d_bool:
                d_start()
                T_cam = False
            elif T_cam and t_bool:
                t_start()
                D_cam = False
            else:
                start_cam = False
    else:
        start_cam = False
else:
    start_cam = False

try:
    start_time = time.time()
    due_time = start_time

    while start_cam and (due_time-start_time <= timer):
        
        if T_cam:
                        
            frame = pipelines_t.wait_for_frames()
            
            pose = frame.get_pose_frame()
            frameset = frame.as_frameset()
                
            f1 = frameset.get_fisheye_frame(1).as_video_frame()
            f2 = frameset.get_fisheye_frame(2).as_video_frame()
    
            left_data = np.asanyarray(f1.get_data())
            right_data = np.asanyarray(f2.get_data())
            
            if pose:
                data = pose.get_pose_data()
                pos_str = "Position: " + str(data.translation)
                velo_str = "Velocity: " + str(data.velocity)
                taccel_str = "Acceleration: "+ str(data.acceleration)
            
            #If data_write is True in start.ini, write IMU data on frames
            if data_write:        
                cv2.putText(left_data, pos_str, (10,30),cv2.FONT_HERSHEY_PLAIN, 0.8, 200)
                cv2.putText(left_data, velo_str, (10,60),cv2.FONT_HERSHEY_PLAIN, 0.8, 200)
                cv2.putText(left_data, taccel_str, (10,90),cv2.FONT_HERSHEY_PLAIN, 0.8, 200)
                    
            cv2.imshow('RealSense_Left', left_data)
            cv2.imshow('RealSense_Right', right_data)

        if D_cam:
            frames = pipelines_d.wait_for_frames()

            a_frame_data = frames[3].as_motion_frame().get_motion_data()
            g_frame_data = frames[2].as_motion_frame().get_motion_data()

            accel = np.asarray([a_frame_data.x, a_frame_data.y, a_frame_data.z], dtype=np.dtype('U25'))
            gyro = np.array([g_frame_data.x, g_frame_data.y, g_frame_data.z], dtype=np.dtype('U25'))
            
            gyro_str = "gyro: x : "+gyro[0]+" y : "+gyro[1]+" z : "+gyro[2]
            accel_str = "accelerometer: x : "+accel[0]+" y : "+accel[1]+" z : "+accel[2]

            depth_frame = frames.get_depth_frame()
            color_frame = frames.get_color_frame()
            
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)

            #If data_write is True in start.ini, write IMU data on frames
            if data_write:
                cv2.putText(color_image, gyro_str, (10,30),cv2.FONT_HERSHEY_PLAIN, 0.80, 200)
                cv2.putText(color_image, accel_str, (10,60),cv2.FONT_HERSHEY_PLAIN, 0.80, 200)
                
            cv2.imshow('RealSense_Color', color_image)
            cv2.imshow('RealSense_Depth', depth_colormap)
        
        #If video is True in start.ini, save video from active cameras
        if video:
            if D_cam:
                colorwriter.write(color_image)
                depthwriter.write(depth_colormap)
            if T_cam:
                right_data=cv2.cvtColor(right_data,cv2.COLOR_GRAY2BGR) 
                rightwriter.write(right_data)
                left_data=cv2.cvtColor(left_data,cv2.COLOR_GRAY2BGR)
                leftwriter.write(left_data)

        #If data_file is True in start.ini, save data from active cameras to data files
        if data_file:
            if D_cam:
                with open('D435i_Data.csv', mode='a') as D_file:
                    D_file_writer = csv.writer(D_file, delimiter=' ', quotechar=' ', quoting=csv.QUOTE_MINIMAL)
                    D_file_writer.writerow([gyro_str])
                    D_file_writer.writerow([accel_str])
            if T_cam:
                with open('T265_Data.csv', mode='a') as T_file:
                    T_file_writer = csv.writer(T_file, delimiter=' ', quotechar=' ', quoting=csv.QUOTE_MINIMAL)
                    T_file_writer.writerow([pos_str])
                    T_file_writer.writerow([velo_str])                
                    T_file_writer.writerow([taccel_str])
        
        due_time=time.time()
        cv2.waitKey(1)

finally:
    if start_cam:
        if D_cam:
            pipelines_d.stop()

        if T_cam:
            pipelines_t.stop()

 

参考：github

#################