-
Notifications
You must be signed in to change notification settings - Fork 1
/
camera_calibration.py
122 lines (99 loc) · 3.93 KB
/
camera_calibration.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
__author__ = 'Douglas'
import numpy as np
import cv2
def estimate_camera(model3D, fidu_XY):
rmat, tvec = calib_camera(model3D, fidu_XY)
RT = np.hstack((rmat, tvec))
projection_matrix = model3D.out_A * RT
return projection_matrix, model3D.out_A, rmat, tvec
def calib_camera(model3D, fidu_XY):
#compute pose using refrence 3D points + query 2D points
ret, rvecs, tvec = cv2.solvePnP(model3D.model_TD, fidu_XY, model3D.out_A, None, None, None, False)
rmat, jacobian = cv2.Rodrigues(rvecs, None)
inside = calc_inside(model3D.out_A, rmat, tvec, model3D.size_U[0], model3D.size_U[1], model3D.model_TD)
if(inside == 0):
tvec = -tvec
t = np.pi
RRz180 = np.asmatrix([np.cos(t), -np.sin(t), 0, np.sin(t), np.cos(t), 0, 0, 0, 1]).reshape((3, 3))
rmat = RRz180*rmat
return rmat, tvec
def get_opengl_matrices(camera_matrix, rmat, tvec, width, height):
projection_matrix = np.asmatrix(np.zeros((4,4)))
near_plane = 0.0001
far_plane = 10000
fx = camera_matrix[0,0]
fy = camera_matrix[1,1]
px = camera_matrix[0,2]
py = camera_matrix[1,2]
projection_matrix[0, 0] = 2.0 * fx / width
projection_matrix[1, 1] = 2.0 * fy / height
projection_matrix[0, 2] = 2.0 * (px / width) - 1.0
projection_matrix[1, 2] = 2.0 * (py / height) - 1.0
projection_matrix[2, 2] = -(far_plane + near_plane) / (far_plane - near_plane)
projection_matrix[3, 2] = -1
projection_matrix[2, 3] = -2.0 * far_plane * near_plane / (far_plane - near_plane)
deg = 180
t = deg*np.pi/180.
RRz=np.asmatrix([np.cos(t), -np.sin(t), 0, np.sin(t), np.cos(t), 0, 0, 0, 1]).reshape((3, 3))
RRy=np.asmatrix([np.cos(t), 0, np.sin(t), 0, 1, 0, -np.sin(t), 0, np.cos(t)]).reshape((3, 3))
rmat=RRz*RRy*rmat
mv = np.asmatrix(np.zeros((4, 4)))
mv[0:3, 0:3] = rmat
mv[0, 3] = tvec[0]
mv[1, 3] = -tvec[1]
mv[2, 3] = -tvec[2]
mv[3, 3] = 1.
return mv, projection_matrix
def extract_frustum(camera_matrix, rmat, tvec, width, height):
mv, proj = get_opengl_matrices(camera_matrix, rmat, tvec, width, height)
clip = proj * mv
frustum = np.asmatrix(np.zeros((6 ,4)))
#/* Extract the numbers for the RIGHT plane */
frustum[0, :] = clip[3, :] - clip[0, :]
#/* Normalize the result */
v = frustum[0, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[0, :] = frustum[0, :]/t
#/* Extract the numbers for the LEFT plane */
frustum[1, :] = clip[3, :] + clip[0, :]
#/* Normalize the result */
v = frustum[1, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[1, :] = frustum[1, :]/t
#/* Extract the BOTTOM plane */
frustum[2, :] = clip[3, :] + clip[1, :]
#/* Normalize the result */
v = frustum[2, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[2, :] = frustum[2, :]/t
#/* Extract the TOP plane */
frustum[3, :] = clip[3, :] - clip[1, :]
#/* Normalize the result */
v = frustum[3, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[3, :] = frustum[3, :]/t
#/* Extract the FAR plane */
frustum[4, :] = clip[3, :] - clip[2, :]
#/* Normalize the result */
v = frustum[4, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[4, :] = frustum[4, :]/t
#/* Extract the NEAR plane */
frustum[5, :] = clip[3, :] + clip[2, :]
#/* Normalize the result */
v = frustum[5, :3]
t = np.sqrt(np.sum(np.multiply(v, v)))
frustum[5, :] = frustum[5, :]/t
return frustum
def calc_inside(camera_matrix, rmat, tvec, width, height, obj_points):
frustum = extract_frustum(camera_matrix, rmat, tvec, width, height)
inside = 0
for point in obj_points:
if(point_in_frustum(point[0], point[1], point[2], frustum) > 0):
inside += 1
return inside
def point_in_frustum(x, y, z, frustum):
for p in range(0, 3):
if(frustum[p, 0] * x + frustum[p, 1] * y + frustum[p, 2] + z + frustum[p, 3] <= 0):
return False
return True