-
Notifications
You must be signed in to change notification settings - Fork 0
/
realsense_intrinsics_module.py
127 lines (90 loc) · 2.63 KB
/
realsense_intrinsics_module.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
##
# @package realsense_intrinsics_module
# Functions for intrisics support
##
# Import OpenCV, librealsense, numpy and logging
import pyrealsense2 as rs
import numpy as np
import cv2
##
# @brief Get profile of frames
# @param image_frames is the frames of the sensor images
# @return profile of an image frame
def get_profile(image_frames):
profile = image_frames.get_profile()
return profile
##
# @brief Get video stream profile of profile
# @param profile is the profile of a sensor
# @return the video stream profile
def get_video_stream_profile(profile):
video_stream_profile = rs.video_stream_profile(profile)
return video_stream_profile
##
# @brief Get the intrinsics of one sensor
# @param video_stream_profile is the stream profile of the video
# @return the intrinsics
def get_intrinsics(video_stream_profile):
intrinsics = video_stream_profile.get_intrinsics()
return intrinsics
##
# @brief Get the width of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the width
def get_width(intrinsics):
width = intrinsics.width
return width
##
# @brief Get the height of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the height
def get_height(intrinsics):
height = intrinsics.height
return height
##
# @brief Get the ppx of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the ppx
def get_ppx(intrinsics):
ppx = intrinsics.ppx
return ppx
##
# @brief Get the ppy of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the ppy
def get_ppy(intrinsics):
ppy = intrinsics.ppy
return ppy
##
# @brief Get the fx of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the fx
def get_fx(intrinsics):
fx = intrinsics.fx
return fx
##
# @brief Get the fy of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the fy
def get_fy(intrinsics):
fy = intrinsics.fy
return fy
##
# @brief Get the coeffs of the frame
# @param intrinsics is the intrinsic parameters of the sensor
# @return the coeffs
def get_coeffs(intrinsics):
coeffs = np.array(intrinsics.coeffs)
coeffs = coeffs[0:4]
return coeffs
##
# @brief Create the intrinsic matrix
# @param intrinsics is the intrinsic parameters of the sensor
# @return the K matrix
def get_K_matrix(intrinsics):
ppx = get_ppx(intrinsics)
ppy = get_ppy(intrinsics)
fx = get_fx(intrinsics)
fy = get_fy(intrinsics)
K = np.array([[fx,0.0,ppx], [0.0,fy,ppy], [0.0,0.0,1.0]])
return K