Skip to content

Commit e72a053

Browse files
vpisarevvpisarev
committed
Merge pull request opencv#10354 from catree:add_python_sample_show_extrinsics
2 parents e43997d + 59ec224 commit e72a053

File tree

2 files changed

+275
-7
lines changed

2 files changed

+275
-7
lines changed

‎samples/data/left_intrinsics.yml

Lines changed: 59 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,75 @@
11
%YAML:1.0
22
---
3+
nframes: 13
34
image_width: 640
45
image_height: 480
56
board_width: 9
67
board_height: 6
7-
square_size: 1.
8+
square_size: 2.5000000372529030e-02
89
aspectRatio: 1.
910
flags: 2
1011
camera_matrix: !!opencv-matrix
1112
rows: 3
1213
cols: 3
1314
dt: d
14-
data: [ 5.3591575307485539e+02, 0., 3.4228314953752817e+02, 0.,
15-
5.3591575307485539e+02, 2.3557082321320789e+02, 0., 0., 1. ]
15+
data: [ 5.3591573396163199e+02, 0., 3.4228315473308373e+02, 0.,
16+
5.3591573396163199e+02, 2.3557082909788173e+02, 0., 0., 1. ]
1617
distortion_coefficients: !!opencv-matrix
1718
rows: 5
1819
cols: 1
1920
dt: d
20-
data: [ -2.6637290673868386e-01, -3.8586722644459073e-02,
21-
1.7831841406179300e-03, -2.8122035403651473e-04,
22-
2.3838760574917545e-01 ]
23-
avg_reprojection_error: 3.9259109564815858e-01
21+
data: [ -2.6637260909660682e-01, -3.8588898922304653e-02,
22+
1.7831947042852964e-03, -2.8122100441115472e-04,
23+
2.3839153080878486e-01 ]
24+
avg_reprojection_error: 3.9259098975581364e-01
25+
per_view_reprojection_errors: !!opencv-matrix
26+
rows: 13
27+
cols: 1
28+
dt: f
29+
data: [ 1.92965463e-01, 1.18204820e+00, 1.73180386e-01,
30+
1.93417311e-01, 1.59574091e-01, 1.79683909e-01, 2.30989486e-01,
31+
2.41952404e-01, 2.96267658e-01, 1.67184874e-01, 2.02002615e-01,
32+
3.81039530e-01, 1.74401343e-01 ]
33+
extrinsic_parameters: !!opencv-matrix
34+
rows: 13
35+
cols: 6
36+
dt: d
37+
data: [ 1.6866673097722978e-01, 2.7567195383689680e-01,
38+
1.3463666677617407e-02, -7.5217911266918208e-02,
39+
-1.0895943925991841e-01, 3.9970206949907272e-01,
40+
4.1331287656496363e-01, 6.4989015618432178e-01,
41+
-1.3371537960145106e+00, -5.8571677080547203e-02,
42+
8.2925805670236566e-02, 3.5381014833230601e-01,
43+
-2.7703695013795054e-01, 1.8693309320100124e-01,
44+
3.5485225341087834e-01, -3.9846501015652937e-02,
45+
-1.0041611109510440e-01, 3.1815947023777164e-01,
46+
-1.1090615673109079e-01, 2.3965970843402720e-01,
47+
-2.1135637810781923e-03, -9.8410654744228568e-02,
48+
-6.7330010965873974e-02, 3.3085237266887146e-01,
49+
-2.9186914919266310e-01, 4.2838824536930098e-01,
50+
1.3127376448141377e+00, 5.8492717894568363e-02,
51+
-1.1531702553211766e-01, 3.1718597226747441e-01,
52+
4.0775746983982769e-01, 3.0372749654555553e-01,
53+
1.6490540383167107e+00, 1.6727077792571535e-01,
54+
-6.5571043573575183e-02, 3.3646131272177648e-01,
55+
1.7933504280050525e-01, 3.4558984092172601e-01,
56+
1.8685292421609112e+00, 1.9533408668697443e-02,
57+
-7.1821904367276174e-02, 3.8942937075181105e-01,
58+
-9.0969163793927624e-02, 4.7978599772080688e-01,
59+
1.7534054022831906e+00, 7.9050417654120575e-02,
60+
-8.7941963150599309e-02, 3.1666076957685929e-01,
61+
2.0297932232462285e-01, -4.2392077549829726e-01,
62+
1.3241327935810543e-01, -6.6346241810532544e-02,
63+
-8.1019305580944570e-02, 2.7830224494208888e-01,
64+
-4.1905731583840156e-01, -4.9969284527936553e-01,
65+
1.3355787183928016e+00, 4.6902734761583582e-02,
66+
-1.1100626108196045e-01, 3.3805630488128308e-01,
67+
-2.3853178487346252e-01, 3.4785724405059820e-01,
68+
1.5307655926865789e+00, 5.0764487316281588e-02,
69+
-1.0259706994505384e-01, 3.2220131320183526e-01,
70+
4.6395663682204152e-01, -2.8347019688901215e-01,
71+
1.2385662249906069e+00, 3.3699309698414767e-02,
72+
-9.1617248179872074e-02, 2.9144614839683858e-01,
73+
-1.6997848268735108e-01, -4.7116903885245226e-01,
74+
1.3459942250907577e+00, 4.5015523494596366e-02,
75+
-1.0817857239600029e-01, 3.1243767202759759e-01 ]

‎samples/python/camera_calibration_show_extrinsics.py

Lines changed: 216 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,216 @@
1+
#!/usr/bin/env python
2+
# -*- coding: utf-8 -*-
3+
4+
from mpl_toolkits.mplot3d import Axes3D
5+
import matplotlib.pyplot as plt
6+
import numpy as np
7+
from matplotlib import cm
8+
from numpy import linspace
9+
import argparse
10+
import cv2 as cv
11+
12+
def inverse_homogeneoux_matrix(M):
13+
R = M[0:3, 0:3]
14+
T = M[0:3, 3]
15+
M_inv = np.identity(4)
16+
M_inv[0:3, 0:3] = R.T
17+
M_inv[0:3, 3] = -(R.T).dot(T)
18+
19+
return M_inv
20+
21+
def transform_to_matplotlib_frame(cMo, X, inverse=False):
22+
M = np.identity(4)
23+
M[1,1] = 0
24+
M[1,2] = 1
25+
M[2,1] = -1
26+
M[2,2] = 0
27+
28+
if inverse:
29+
return M.dot(inverse_homogeneoux_matrix(cMo).dot(X))
30+
else:
31+
return M.dot(cMo.dot(X))
32+
33+
def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_axis=False):
34+
fx = camera_matrix[0,0]
35+
fy = camera_matrix[1,1]
36+
focal = 2 / (fx + fy)
37+
f_scale = scale_focal * focal
38+
39+
# draw image plane
40+
X_img_plane = np.ones((4,5))
41+
X_img_plane[0:3,0] = [-width, height, f_scale]
42+
X_img_plane[0:3,1] = [width, height, f_scale]
43+
X_img_plane[0:3,2] = [width, -height, f_scale]
44+
X_img_plane[0:3,3] = [-width, -height, f_scale]
45+
X_img_plane[0:3,4] = [-width, height, f_scale]
46+
47+
# draw triangle above the image plane
48+
X_triangle = np.ones((4,3))
49+
X_triangle[0:3,0] = [-width, -height, f_scale]
50+
X_triangle[0:3,1] = [0, -2*height, f_scale]
51+
X_triangle[0:3,2] = [width, -height, f_scale]
52+
53+
# draw camera
54+
X_center1 = np.ones((4,2))
55+
X_center1[0:3,0] = [0, 0, 0]
56+
X_center1[0:3,1] = [-width, height, f_scale]
57+
58+
X_center2 = np.ones((4,2))
59+
X_center2[0:3,0] = [0, 0, 0]
60+
X_center2[0:3,1] = [width, height, f_scale]
61+
62+
X_center3 = np.ones((4,2))
63+
X_center3[0:3,0] = [0, 0, 0]
64+
X_center3[0:3,1] = [width, -height, f_scale]
65+
66+
X_center4 = np.ones((4,2))
67+
X_center4[0:3,0] = [0, 0, 0]
68+
X_center4[0:3,1] = [-width, -height, f_scale]
69+
70+
# draw camera frame axis
71+
X_frame1 = np.ones((4,2))
72+
X_frame1[0:3,0] = [0, 0, 0]
73+
X_frame1[0:3,1] = [f_scale/2, 0, 0]
74+
75+
X_frame2 = np.ones((4,2))
76+
X_frame2[0:3,0] = [0, 0, 0]
77+
X_frame2[0:3,1] = [0, f_scale/2, 0]
78+
79+
X_frame3 = np.ones((4,2))
80+
X_frame3[0:3,0] = [0, 0, 0]
81+
X_frame3[0:3,1] = [0, 0, f_scale/2]
82+
83+
if draw_frame_axis:
84+
return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4, X_frame1, X_frame2, X_frame3]
85+
else:
86+
return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4]
87+
88+
def create_board_model(extrinsics, board_width, board_height, square_size, draw_frame_axis=False):
89+
width = board_width*square_size
90+
height = board_height*square_size
91+
92+
# draw calibration board
93+
X_board = np.ones((4,5))
94+
X_board_cam = np.ones((extrinsics.shape[0],4,5))
95+
X_board[0:3,0] = [0,0,0]
96+
X_board[0:3,1] = [width,0,0]
97+
X_board[0:3,2] = [width,height,0]
98+
X_board[0:3,3] = [0,height,0]
99+
X_board[0:3,4] = [0,0,0]
100+
101+
# draw board frame axis
102+
X_frame1 = np.ones((4,2))
103+
X_frame1[0:3,0] = [0, 0, 0]
104+
X_frame1[0:3,1] = [height/2, 0, 0]
105+
106+
X_frame2 = np.ones((4,2))
107+
X_frame2[0:3,0] = [0, 0, 0]
108+
X_frame2[0:3,1] = [0, height/2, 0]
109+
110+
X_frame3 = np.ones((4,2))
111+
X_frame3[0:3,0] = [0, 0, 0]
112+
X_frame3[0:3,1] = [0, 0, height/2]
113+
114+
if draw_frame_axis:
115+
return [X_board, X_frame1, X_frame2, X_frame3]
116+
else:
117+
return [X_board]
118+
119+
def draw_camera_boards(ax, camera_matrix, cam_width, cam_height, scale_focal,
120+
extrinsics, board_width, board_height, square_size,
121+
patternCentric):
122+
min_values = np.zeros((3,1))
123+
min_values = np.inf
124+
max_values = np.zeros((3,1))
125+
max_values = -np.inf
126+
127+
if patternCentric:
128+
X_moving = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal)
129+
X_static = create_board_model(extrinsics, board_width, board_height, square_size)
130+
else:
131+
X_static = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal, True)
132+
X_moving = create_board_model(extrinsics, board_width, board_height, square_size)
133+
134+
cm_subsection = linspace(0.0, 1.0, extrinsics.shape[0])
135+
colors = [ cm.jet(x) for x in cm_subsection ]
136+
137+
for i in range(len(X_static)):
138+
X = np.zeros(X_static[i].shape)
139+
for j in range(X_static[i].shape[1]):
140+
X[:,j] = transform_to_matplotlib_frame(np.eye(4), X_static[i][:,j])
141+
ax.plot3D(X[0,:], X[1,:], X[2,:], color='r')
142+
min_values = np.minimum(min_values, X[0:3,:].min(1))
143+
max_values = np.maximum(max_values, X[0:3,:].max(1))
144+
145+
for idx in range(extrinsics.shape[0]):
146+
R, _ = cv.Rodrigues(extrinsics[idx,0:3])
147+
cMo = np.eye(4,4)
148+
cMo[0:3,0:3] = R
149+
cMo[0:3,3] = extrinsics[idx,3:6]
150+
for i in range(len(X_moving)):
151+
X = np.zeros(X_moving[i].shape)
152+
for j in range(X_moving[i].shape[1]):
153+
X[0:4,j] = transform_to_matplotlib_frame(cMo, X_moving[i][0:4,j], patternCentric)
154+
ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[idx])
155+
min_values = np.minimum(min_values, X[0:3,:].min(1))
156+
max_values = np.maximum(max_values, X[0:3,:].max(1))
157+
158+
return min_values, max_values
159+
160+
def main():
161+
parser = argparse.ArgumentParser(description='Plot camera calibration extrinsics.',
162+
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
163+
parser.add_argument('--calibration', type=str, default="../data/left_intrinsics.yml",
164+
help='YAML camera calibration file.')
165+
parser.add_argument('--cam_width', type=float, default=0.064/2,
166+
help='Width/2 of the displayed camera.')
167+
parser.add_argument('--cam_height', type=float, default=0.048/2,
168+
help='Height/2 of the displayed camera.')
169+
parser.add_argument('--scale_focal', type=float, default=40,
170+
help='Value to scale the focal length.')
171+
parser.add_argument('--patternCentric', action='store_true',
172+
help='The calibration board is static and the camera is moving.')
173+
args = parser.parse_args()
174+
175+
fs = cv.FileStorage(args.calibration, cv.FILE_STORAGE_READ)
176+
board_width = int(fs.getNode('board_width').real())
177+
board_height = int(fs.getNode('board_height').real())
178+
square_size = fs.getNode('square_size').real()
179+
camera_matrix = fs.getNode('camera_matrix').mat()
180+
extrinsics = fs.getNode('extrinsic_parameters').mat()
181+
182+
fig = plt.figure()
183+
ax = fig.gca(projection='3d')
184+
ax.set_aspect("equal")
185+
186+
cam_width = args.cam_width
187+
cam_height = args.cam_height
188+
scale_focal = args.scale_focal
189+
min_values, max_values = draw_camera_boards(ax, camera_matrix, cam_width, cam_height,
190+
scale_focal, extrinsics, board_width,
191+
board_height, square_size, args.patternCentric)
192+
193+
X_min = min_values[0]
194+
X_max = max_values[0]
195+
Y_min = min_values[1]
196+
Y_max = max_values[1]
197+
Z_min = min_values[2]
198+
Z_max = max_values[2]
199+
max_range = np.array([X_max-X_min, Y_max-Y_min, Z_max-Z_min]).max() / 2.0
200+
201+
mid_x = (X_max+X_min) * 0.5
202+
mid_y = (Y_max+Y_min) * 0.5
203+
mid_z = (Z_max+Z_min) * 0.5
204+
ax.set_xlim(mid_x - max_range, mid_x + max_range)
205+
ax.set_ylim(mid_y - max_range, mid_y + max_range)
206+
ax.set_zlim(mid_z - max_range, mid_z + max_range)
207+
208+
ax.set_xlabel('x')
209+
ax.set_ylabel('z')
210+
ax.set_zlabel('-y')
211+
ax.set_title('Extrinsic Parameters Visualization')
212+
213+
plt.show()
214+
215+
if __name__ == "__main__":
216+
main()

0 commit comments

Comments
 (0)