Pose and Frames Together

[jdanielhoyos]

Hello everyone. How to call the frames and pose in the same script? I've been working on it for a while and I does not work.
Thank you!

[jdanielhoyos]

I solved it, theres the code if it helps.

`from future import print_function

import pyrealsense2 as rs
import cv2
import numpy as np
from math import tan, pi
import scipy
import pdb
from scipy.io import savemat
def get_extrinsics(src, dst):
extrinsics = src.get_extrinsics_to(dst)
R = np.reshape(extrinsics.rotation, [3,3]).T
T = np.array(extrinsics.translation)
return (R, T)
def camera_matrix(intrinsics):
return np.array([[intrinsics.fx, 0, intrinsics.ppx],
[ 0, intrinsics.fy, intrinsics.ppy],
[ 0, 0, 1]])
def fisheye_distortion(intrinsics):
return np.array(intrinsics.coeffs[:4])
from threading import Lock
frame_mutex = Lock()
frame_data = {"left" : None,
"right" : None,
"timestamp_ms" : None
}

pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_all_streams
pipe.start(cfg)
from matplotlib import pyplot as plt
try:
window_size = 5
min_disp = 0
# must be divisible by 16
num_disp = 112 - min_disp
max_disp = min_disp + num_disp
# Retreive the stream and intrinsic properties for both cameras
profiles = pipe.get_active_profile()
streams = {"left" : profiles.get_stream(rs.stream.fisheye, 1).as_video_stream_profile(),
"right" : profiles.get_stream(rs.stream.fisheye, 2).as_video_stream_profile()}
intrinsics = {"left" : streams["left"].get_intrinsics(),
"right" : streams["right"].get_intrinsics()}

# Translate the intrinsics from librealsense into OpenCV
K_left  = camera_matrix(intrinsics["left"])
D_left  = fisheye_distortion(intrinsics["left"])
K_right = camera_matrix(intrinsics["right"])
D_right = fisheye_distortion(intrinsics["right"])
(width, height) = (intrinsics["left"].width, intrinsics["left"].height)

# Get the relative extrinsics between the left and right camera
(R, T) = get_extrinsics(streams["left"], streams["right"])

stereo_fov_rad = 90 * (pi/180)  # 90 degree desired fov
stereo_height_px = 800          # 800x800 pixel stereo output
stereo_focal_px = stereo_height_px/2 / tan(stereo_fov_rad/2)

R_left = np.eye(3)
R_right = R

stereo_width_px = stereo_height_px + max_disp
stereo_size = (stereo_width_px, stereo_height_px)
stereo_cx = (stereo_height_px - 1)/2 + max_disp
stereo_cy = (stereo_height_px - 1)/2

# Construct the left and right projection matrices, the only difference is
# that the right projection matrix should have a shift along the x axis of
# baseline*focal_length
P_left = np.array([[stereo_focal_px, 0, stereo_cx, 0],
                   [0, stereo_focal_px, stereo_cy, 0],
                   [0,               0,         1, 0]])
P_right = P_left.copy()
P_right[0][3] = T[0]*stereo_focal_px
Q = np.array([[1, 0,       0, -(stereo_cx - max_disp)],
              [0, 1,       0, -stereo_cy],
              [0, 0,       0, stereo_focal_px],
              [0, 0, -1/T[0], 0]])

# Create an undistortion map for the left and right camera which applies the
# rectification and undoes the camera distortion. This only has to be done
# once
m1type = cv2.CV_32FC1
(lm1, lm2) = cv2.fisheye.initUndistortRectifyMap(K_left, D_left, R_left, P_left, stereo_size, m1type)
(rm1, rm2) = cv2.fisheye.initUndistortRectifyMap(K_right, D_right, R_right, P_right, stereo_size, m1type)
undistort_rectify = {"left"  : (lm1, lm2),
                     "right" : (rm1, rm2)}
mode = "stack"
fra=1
while fra==1:
 
    fs  =  pipe.wait_for_frames()
    fs.is_frameset()
    pose = fs.get_pose_frame()
    #pdb.set_trace() 
    framesleft  = fs.get_fisheye_frame(1)
    left  =  framesleft.get_data()
    left_data = np.asanyarray(left)
    framesright  = fs.get_fisheye_frame(2)
    right  =  framesright.get_data()
    right_data = np.asanyarray(right)
    if pose:
        # Print some of the pose data to the terminal
        data = pose.get_pose_data()

    
    # Check if the camera has acquired any frames
    # If frames are ready to process
    if fs.is_frame():
        
        # Hold the mutex only long enough to copy the stereo frames
        

        

        # Undistort and crop the center of the frames
        center_undistorted = {"left" : cv2.remap(src = left_data,
                                      map1 = undistort_rectify["left"][0],
                                      map2 = undistort_rectify["left"][1],
                                      interpolation = cv2.INTER_LINEAR),
                              "right" : cv2.remap(src = right_data,
                                      map1 = undistort_rectify["right"][0],
                                      map2 = undistort_rectify["right"][1],
                                      interpolation = cv2.INTER_LINEAR)}
        left=cv2.remap(src = left_data,map1 = undistort_rectify["left"][0],map2 = undistort_rectify["left"][1],interpolation = cv2.INTER_LINEAR)
        right=cv2.remap(src = right_data,map1 = undistort_rectify["right"][0],map2 = undistort_rectify["right"][1],interpolation = cv2.INTER_LINEAR)
        #plt.imsave('left.png',cv2.remap(src = frame_copy["left"],map1 = undistort_rectify["left"][0],map2 = undistort_rectify["left"][1],interpolation = cv2.INTER_LINEAR))
        #plt.imsave('right.png',cv2.remap(src = frame_copy["right"],map1 = undistort_rectify["right"][0],map2 = undistort_rectify["right"][1],interpolation = cv2.INTER_LINEAR))     
        
        left = left.astype('float64')
        right = right.astype('float64')
        scipy.io.savemat('left.mat', {'mydata': left[:,:]})
        scipy.io.savemat('right.mat', {'mydata': right[:,:]})





        scipy.io.savemat('IMU.mat', {'mydata': np.array([data.translation.x, data.translation.y, data.translation.z, data.rotation.x, data.rotation.y, data.rotation.z, data.rotation.w])})

finally:
pipe.stop()
`