6 from __future__
import print_function
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. 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 18 Getting started with python3, OpenCV and T265 on Ubuntu 16.04: 20 First, set up the virtual enviroment: 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 28 Then, for every new terminal: 30 $ source py3librs/bin/activate # Activate the virtual environment 31 $ python3 t265_stereo.py # Run the example 35 import pyrealsense2
as rs
40 from math
import tan, pi
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 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: 51 https://docs.opencv.org/3.4/db/d58/group__calib3d__fisheye.html 55 Returns R, T transform from src to dst 58 extrinsics = src.get_extrinsics_to(dst)
59 R = np.reshape(extrinsics.rotation, [3,3]).T
60 T = np.array(extrinsics.translation)
64 Returns a camera matrix K from librealsense intrinsics 67 return np.array([[intrinsics.fx, 0, intrinsics.ppx],
68 [ 0, intrinsics.fy, intrinsics.ppy],
72 Returns the fisheye distortion from librealsense intrinsics 75 return np.array(intrinsics.coeffs[:4])
78 from threading
import Lock
80 frame_data = {
"left" :
None,
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 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()
113 pipe.start(cfg, callback)
117 WINDOW_TITLE =
'Realsense' 118 cv2.namedWindow(WINDOW_TITLE, cv2.WINDOW_NORMAL)
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,
131 P1 = 8*3*window_size**2,
132 P2 = 32*3*window_size**2,
134 uniquenessRatio = 10,
135 speckleWindowSize = 100,
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()}
146 print(
"Left camera:", intrinsics[
"left"])
147 print(
"Right camera:", intrinsics[
"right"])
154 (width, height) = (intrinsics[
"left"].width, intrinsics[
"left"].height)
173 stereo_fov_rad = 90 * (pi/180)
174 stereo_height_px = 300
175 stereo_focal_px = stereo_height_px/2 / tan(stereo_fov_rad/2)
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
193 P_left = np.array([[stereo_focal_px, 0, stereo_cx, 0],
194 [0, stereo_focal_px, stereo_cy, 0],
196 P_right = P_left.copy()
197 P_right[0][3] = T[0]*stereo_focal_px
201 Q = np.array([[1, 0, 0, -(stereo_cx - max_disp)],
202 [0, 1, 0, -stereo_cy],
203 [0, 0, 0, stereo_focal_px],
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)}
218 frame_mutex.acquire()
219 valid = frame_data[
"timestamp_ms"]
is not None 220 frame_mutex.release()
225 frame_mutex.acquire()
226 frame_copy = {
"left" : frame_data[
"left"].
copy(),
227 "right" : frame_data[
"right"].
copy()}
228 frame_mutex.release()
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)}
241 disparity = stereo.compute(center_undistorted[
"left"], center_undistorted[
"right"]).astype(np.float32) / 16.0
244 disparity = disparity[:,max_disp:]
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)
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)
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:
def fisheye_distortion(intrinsics)
def camera_matrix(intrinsics)
static std::string print(const transformation &tf)
def get_extrinsics(src, dst)
void copy(void *dst, void const *src, size_t size)