t265_stereo.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 # -*- coding: utf-8 -*-
3 ## License: Apache 2.0. See LICENSE file in root directory.
4 ## Copyright(c) 2019 Intel Corporation. All Rights Reserved.
5 # Python 2/3 compatibility
6 from __future__ import print_function
7 
8 """
9 This example shows how to use T265 intrinsics and extrinsics in OpenCV to
10 asynchronously compute depth maps from T265 fisheye images on the host.
11 
12 T265 is not a depth camera and the quality of passive-only depth options will
13 always be limited compared to (e.g.) the D4XX series cameras. However, T265 does
14 have two global shutter cameras in a stereo configuration, and in this example
15 we show how to set up OpenCV to undistort the images and compute stereo depth
16 from them.
17 
18 Getting started with python3, OpenCV and T265 on Ubuntu 16.04:
19 
20 First, set up the virtual enviroment:
21 
22 $ apt-get install python3-venv # install python3 built in venv support
23 $ python3 -m venv py3librs # create a virtual environment in pylibrs
24 $ source py3librs/bin/activate # activate the venv, do this from every terminal
25 $ pip install opencv-python # install opencv 4.1 in the venv
26 $ pip install pyrealsense2 # install librealsense python bindings
27 
28 Then, for every new terminal:
29 
30 $ source py3librs/bin/activate # Activate the virtual environment
31 $ python3 t265_stereo.py # Run the example
32 """
33 
34 # First import the library
35 import pyrealsense2 as rs
36 
37 # Import OpenCV and numpy
38 import cv2
39 import numpy as np
40 from math import tan, pi
41 
42 """
43 In this section, we will set up the functions that will translate the camera
44 intrinsics and extrinsics from librealsense into parameters that can be used
45 with OpenCV.
46 
47 The T265 uses very wide angle lenses, so the distortion is modeled using a four
48 parameter distortion model known as Kanalla-Brandt. OpenCV supports this
49 distortion model in their "fisheye" module, more details can be found here:
50 
51 https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html
52 """
53 
54 """
55 Returns R, T transform from src to dst
56 """
57 def get_extrinsics(src, dst):
58  extrinsics = src.get_extrinsics_to(dst)
59  R = np.reshape(extrinsics.rotation, [3,3]).T
60  T = np.array(extrinsics.translation)
61  return (R, T)
62 
63 """
64 Returns a camera matrix K from librealsense intrinsics
65 """
66 def camera_matrix(intrinsics):
67  return np.array([[intrinsics.fx, 0, intrinsics.ppx],
68  [ 0, intrinsics.fy, intrinsics.ppy],
69  [ 0, 0, 1]])
70 
71 """
72 Returns the fisheye distortion from librealsense intrinsics
73 """
74 def fisheye_distortion(intrinsics):
75  return np.array(intrinsics.coeffs[:4])
76 
77 # Set up a mutex to share data between threads
78 from threading import Lock
79 frame_mutex = Lock()
80 frame_data = {"left" : None,
81  "right" : None,
82  "timestamp_ms" : None
83  }
84 
85 """
86 This callback is called on a separate thread, so we must use a mutex
87 to ensure that data is synchronized properly. We should also be
88 careful not to do much work on this thread to avoid data backing up in the
89 callback queue.
90 """
91 def callback(frame):
92  global frame_data
93  if frame.is_frameset():
94  frameset = frame.as_frameset()
95  f1 = frameset.get_fisheye_frame(1).as_video_frame()
96  f2 = frameset.get_fisheye_frame(2).as_video_frame()
97  left_data = np.asanyarray(f1.get_data())
98  right_data = np.asanyarray(f2.get_data())
99  ts = frameset.get_timestamp()
100  frame_mutex.acquire()
101  frame_data["left"] = left_data
102  frame_data["right"] = right_data
103  frame_data["timestamp_ms"] = ts
104  frame_mutex.release()
105 
106 # Declare RealSense pipeline, encapsulating the actual device and sensors
107 pipe = rs.pipeline()
108 
109 # Build config object and stream everything
110 cfg = rs.config()
111 
112 # Start streaming with our callback
113 pipe.start(cfg, callback)
114 
115 try:
116  # Set up an OpenCV window to visualize the results
117  WINDOW_TITLE = 'Realsense'
118  cv2.namedWindow(WINDOW_TITLE, cv2.WINDOW_NORMAL)
119 
120  # Configure the OpenCV stereo algorithm. See
121  # https://docs.opencv.org/3.4/d2/d85/classcv_1_1StereoSGBM.html for a
122  # description of the parameters
123  window_size = 5
124  min_disp = 0
125  # must be divisible by 16
126  num_disp = 112 - min_disp
127  max_disp = min_disp + num_disp
128  stereo = cv2.StereoSGBM_create(minDisparity = min_disp,
129  numDisparities = num_disp,
130  blockSize = 16,
131  P1 = 8*3*window_size**2,
132  P2 = 32*3*window_size**2,
133  disp12MaxDiff = 1,
134  uniquenessRatio = 10,
135  speckleWindowSize = 100,
136  speckleRange = 32)
137 
138  # Retreive the stream and intrinsic properties for both cameras
139  profiles = pipe.get_active_profile()
140  streams = {"left" : profiles.get_stream(rs.stream.fisheye, 1).as_video_stream_profile(),
141  "right" : profiles.get_stream(rs.stream.fisheye, 2).as_video_stream_profile()}
142  intrinsics = {"left" : streams["left"].get_intrinsics(),
143  "right" : streams["right"].get_intrinsics()}
144 
145  # Print information about both cameras
146  print("Left camera:", intrinsics["left"])
147  print("Right camera:", intrinsics["right"])
148 
149  # Translate the intrinsics from librealsense into OpenCV
150  K_left = camera_matrix(intrinsics["left"])
151  D_left = fisheye_distortion(intrinsics["left"])
152  K_right = camera_matrix(intrinsics["right"])
153  D_right = fisheye_distortion(intrinsics["right"])
154  (width, height) = (intrinsics["left"].width, intrinsics["left"].height)
155 
156  # Get the relative extrinsics between the left and right camera
157  (R, T) = get_extrinsics(streams["left"], streams["right"])
158 
159  # We need to determine what focal length our undistorted images should have
160  # in order to set up the camera matrices for initUndistortRectifyMap. We
161  # could use stereoRectify, but here we show how to derive these projection
162  # matrices from the calibration and a desired height and field of view
163 
164  # We calculate the undistorted focal length:
165  #
166  # h
167  # -----------------
168  # \ | /
169  # \ | f /
170  # \ | /
171  # \ fov /
172  # \|/
173  stereo_fov_rad = 90 * (pi/180) # 90 degree desired fov
174  stereo_height_px = 300 # 300x300 pixel stereo output
175  stereo_focal_px = stereo_height_px/2 / tan(stereo_fov_rad/2)
176 
177  # We set the left rotation to identity and the right rotation
178  # the rotation between the cameras
179  R_left = np.eye(3)
180  R_right = R
181 
182  # The stereo algorithm needs max_disp extra pixels in order to produce valid
183  # disparity on the desired output region. This changes the width, but the
184  # center of projection should be on the center of the cropped image
185  stereo_width_px = stereo_height_px + max_disp
186  stereo_size = (stereo_width_px, stereo_height_px)
187  stereo_cx = (stereo_height_px - 1)/2 + max_disp
188  stereo_cy = (stereo_height_px - 1)/2
189 
190  # Construct the left and right projection matrices, the only difference is
191  # that the right projection matrix should have a shift along the x axis of
192  # baseline*focal_length
193  P_left = np.array([[stereo_focal_px, 0, stereo_cx, 0],
194  [0, stereo_focal_px, stereo_cy, 0],
195  [0, 0, 1, 0]])
196  P_right = P_left.copy()
197  P_right[0][3] = T[0]*stereo_focal_px
198 
199  # Construct Q for use with cv2.reprojectImageTo3D. Subtract max_disp from x
200  # since we will crop the disparity later
201  Q = np.array([[1, 0, 0, -(stereo_cx - max_disp)],
202  [0, 1, 0, -stereo_cy],
203  [0, 0, 0, stereo_focal_px],
204  [0, 0, -1/T[0], 0]])
205 
206  # Create an undistortion map for the left and right camera which applies the
207  # rectification and undoes the camera distortion. This only has to be done
208  # once
209  m1type = cv2.CV_32FC1
210  (lm1, lm2) = cv2.fisheye.initUndistortRectifyMap(K_left, D_left, R_left, P_left, stereo_size, m1type)
211  (rm1, rm2) = cv2.fisheye.initUndistortRectifyMap(K_right, D_right, R_right, P_right, stereo_size, m1type)
212  undistort_rectify = {"left" : (lm1, lm2),
213  "right" : (rm1, rm2)}
214 
215  mode = "stack"
216  while True:
217  # Check if the camera has acquired any frames
218  frame_mutex.acquire()
219  valid = frame_data["timestamp_ms"] is not None
220  frame_mutex.release()
221 
222  # If frames are ready to process
223  if valid:
224  # Hold the mutex only long enough to copy the stereo frames
225  frame_mutex.acquire()
226  frame_copy = {"left" : frame_data["left"].copy(),
227  "right" : frame_data["right"].copy()}
228  frame_mutex.release()
229 
230  # Undistort and crop the center of the frames
231  center_undistorted = {"left" : cv2.remap(src = frame_copy["left"],
232  map1 = undistort_rectify["left"][0],
233  map2 = undistort_rectify["left"][1],
234  interpolation = cv2.INTER_LINEAR),
235  "right" : cv2.remap(src = frame_copy["right"],
236  map1 = undistort_rectify["right"][0],
237  map2 = undistort_rectify["right"][1],
238  interpolation = cv2.INTER_LINEAR)}
239 
240  # compute the disparity on the center of the frames and convert it to a pixel disparity (divide by DISP_SCALE=16)
241  disparity = stereo.compute(center_undistorted["left"], center_undistorted["right"]).astype(np.float32) / 16.0
242 
243  # re-crop just the valid part of the disparity
244  disparity = disparity[:,max_disp:]
245 
246  # convert disparity to 0-255 and color it
247  disp_vis = 255*(disparity - min_disp)/ num_disp
248  disp_color = cv2.applyColorMap(cv2.convertScaleAbs(disp_vis,1), cv2.COLORMAP_JET)
249  color_image = cv2.cvtColor(center_undistorted["left"][:,max_disp:], cv2.COLOR_GRAY2RGB)
250 
251  if mode == "stack":
252  cv2.imshow(WINDOW_TITLE, np.hstack((color_image, disp_color)))
253  if mode == "overlay":
254  ind = disparity >= min_disp
255  color_image[ind, 0] = disp_color[ind, 0]
256  color_image[ind, 1] = disp_color[ind, 1]
257  color_image[ind, 2] = disp_color[ind, 2]
258  cv2.imshow(WINDOW_TITLE, color_image)
259  key = cv2.waitKey(1)
260  if key == ord('s'): mode = "stack"
261  if key == ord('o'): mode = "overlay"
262  if key == ord('q') or cv2.getWindowProperty(WINDOW_TITLE, cv2.WND_PROP_VISIBLE) < 1:
263  break
264 finally:
265  pipe.stop()
def fisheye_distortion(intrinsics)
Definition: t265_stereo.py:74
def camera_matrix(intrinsics)
Definition: t265_stereo.py:66
def callback(frame)
Definition: t265_stereo.py:91
static std::string print(const transformation &tf)
def get_extrinsics(src, dst)
Definition: t265_stereo.py:57
void copy(void *dst, void const *src, size_t size)
Definition: types.cpp:836


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:11