mirror of
https://github.com/opencv/opencv.git
synced 2024-11-27 20:50:25 +08:00
Merge pull request #10354 from catree:add_python_sample_show_extrinsics
This commit is contained in:
Vadim Pisarevsky
commit
e72a053cca
@ -1,23 +1,75 @@
|
||||
%YAML:1.0
|
||||
---
|
||||
nframes: 13
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
board_width: 9
|
||||
board_height: 6
|
||||
square_size: 1.
|
||||
square_size: 2.5000000372529030e-02
|
||||
aspectRatio: 1.
|
||||
flags: 2
|
||||
camera_matrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 5.3591575307485539e+02, 0., 3.4228314953752817e+02, 0.,
|
||||
5.3591575307485539e+02, 2.3557082321320789e+02, 0., 0., 1. ]
|
||||
data: [ 5.3591573396163199e+02, 0., 3.4228315473308373e+02, 0.,
|
||||
5.3591573396163199e+02, 2.3557082909788173e+02, 0., 0., 1. ]
|
||||
distortion_coefficients: !!opencv-matrix
|
||||
rows: 5
|
||||
cols: 1
|
||||
dt: d
|
||||
data: [ -2.6637290673868386e-01, -3.8586722644459073e-02,
|
||||
1.7831841406179300e-03, -2.8122035403651473e-04,
|
||||
2.3838760574917545e-01 ]
|
||||
avg_reprojection_error: 3.9259109564815858e-01
|
||||
data: [ -2.6637260909660682e-01, -3.8588898922304653e-02,
|
||||
1.7831947042852964e-03, -2.8122100441115472e-04,
|
||||
2.3839153080878486e-01 ]
|
||||
avg_reprojection_error: 3.9259098975581364e-01
|
||||
per_view_reprojection_errors: !!opencv-matrix
|
||||
rows: 13
|
||||
cols: 1
|
||||
dt: f
|
||||
data: [ 1.92965463e-01, 1.18204820e+00, 1.73180386e-01,
|
||||
1.93417311e-01, 1.59574091e-01, 1.79683909e-01, 2.30989486e-01,
|
||||
2.41952404e-01, 2.96267658e-01, 1.67184874e-01, 2.02002615e-01,
|
||||
3.81039530e-01, 1.74401343e-01 ]
|
||||
extrinsic_parameters: !!opencv-matrix
|
||||
rows: 13
|
||||
cols: 6
|
||||
dt: d
|
||||
data: [ 1.6866673097722978e-01, 2.7567195383689680e-01,
|
||||
1.3463666677617407e-02, -7.5217911266918208e-02,
|
||||
-1.0895943925991841e-01, 3.9970206949907272e-01,
|
||||
4.1331287656496363e-01, 6.4989015618432178e-01,
|
||||
-1.3371537960145106e+00, -5.8571677080547203e-02,
|
||||
8.2925805670236566e-02, 3.5381014833230601e-01,
|
||||
-2.7703695013795054e-01, 1.8693309320100124e-01,
|
||||
3.5485225341087834e-01, -3.9846501015652937e-02,
|
||||
-1.0041611109510440e-01, 3.1815947023777164e-01,
|
||||
-1.1090615673109079e-01, 2.3965970843402720e-01,
|
||||
-2.1135637810781923e-03, -9.8410654744228568e-02,
|
||||
-6.7330010965873974e-02, 3.3085237266887146e-01,
|
||||
-2.9186914919266310e-01, 4.2838824536930098e-01,
|
||||
1.3127376448141377e+00, 5.8492717894568363e-02,
|
||||
-1.1531702553211766e-01, 3.1718597226747441e-01,
|
||||
4.0775746983982769e-01, 3.0372749654555553e-01,
|
||||
1.6490540383167107e+00, 1.6727077792571535e-01,
|
||||
-6.5571043573575183e-02, 3.3646131272177648e-01,
|
||||
1.7933504280050525e-01, 3.4558984092172601e-01,
|
||||
1.8685292421609112e+00, 1.9533408668697443e-02,
|
||||
-7.1821904367276174e-02, 3.8942937075181105e-01,
|
||||
-9.0969163793927624e-02, 4.7978599772080688e-01,
|
||||
1.7534054022831906e+00, 7.9050417654120575e-02,
|
||||
-8.7941963150599309e-02, 3.1666076957685929e-01,
|
||||
2.0297932232462285e-01, -4.2392077549829726e-01,
|
||||
1.3241327935810543e-01, -6.6346241810532544e-02,
|
||||
-8.1019305580944570e-02, 2.7830224494208888e-01,
|
||||
-4.1905731583840156e-01, -4.9969284527936553e-01,
|
||||
1.3355787183928016e+00, 4.6902734761583582e-02,
|
||||
-1.1100626108196045e-01, 3.3805630488128308e-01,
|
||||
-2.3853178487346252e-01, 3.4785724405059820e-01,
|
||||
1.5307655926865789e+00, 5.0764487316281588e-02,
|
||||
-1.0259706994505384e-01, 3.2220131320183526e-01,
|
||||
4.6395663682204152e-01, -2.8347019688901215e-01,
|
||||
1.2385662249906069e+00, 3.3699309698414767e-02,
|
||||
-9.1617248179872074e-02, 2.9144614839683858e-01,
|
||||
-1.6997848268735108e-01, -4.7116903885245226e-01,
|
||||
1.3459942250907577e+00, 4.5015523494596366e-02,
|
||||
-1.0817857239600029e-01, 3.1243767202759759e-01 ]
|
||||
|
||||
216
samples/python/camera_calibration_show_extrinsics.py
Executable file
216
samples/python/camera_calibration_show_extrinsics.py
Executable file
@ -0,0 +1,216 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from matplotlib import cm
|
||||
from numpy import linspace
|
||||
import argparse
|
||||
import cv2 as cv
|
||||
|
||||
def inverse_homogeneoux_matrix(M):
|
||||
R = M[0:3, 0:3]
|
||||
T = M[0:3, 3]
|
||||
M_inv = np.identity(4)
|
||||
M_inv[0:3, 0:3] = R.T
|
||||
M_inv[0:3, 3] = -(R.T).dot(T)
|
||||
|
||||
return M_inv
|
||||
|
||||
def transform_to_matplotlib_frame(cMo, X, inverse=False):
|
||||
M = np.identity(4)
|
||||
M[1,1] = 0
|
||||
M[1,2] = 1
|
||||
M[2,1] = -1
|
||||
M[2,2] = 0
|
||||
|
||||
if inverse:
|
||||
return M.dot(inverse_homogeneoux_matrix(cMo).dot(X))
|
||||
else:
|
||||
return M.dot(cMo.dot(X))
|
||||
|
||||
def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_axis=False):
|
||||
fx = camera_matrix[0,0]
|
||||
fy = camera_matrix[1,1]
|
||||
focal = 2 / (fx + fy)
|
||||
f_scale = scale_focal * focal
|
||||
|
||||
# draw image plane
|
||||
X_img_plane = np.ones((4,5))
|
||||
X_img_plane[0:3,0] = [-width, height, f_scale]
|
||||
X_img_plane[0:3,1] = [width, height, f_scale]
|
||||
X_img_plane[0:3,2] = [width, -height, f_scale]
|
||||
X_img_plane[0:3,3] = [-width, -height, f_scale]
|
||||
X_img_plane[0:3,4] = [-width, height, f_scale]
|
||||
|
||||
# draw triangle above the image plane
|
||||
X_triangle = np.ones((4,3))
|
||||
X_triangle[0:3,0] = [-width, -height, f_scale]
|
||||
X_triangle[0:3,1] = [0, -2*height, f_scale]
|
||||
X_triangle[0:3,2] = [width, -height, f_scale]
|
||||
|
||||
# draw camera
|
||||
X_center1 = np.ones((4,2))
|
||||
X_center1[0:3,0] = [0, 0, 0]
|
||||
X_center1[0:3,1] = [-width, height, f_scale]
|
||||
|
||||
X_center2 = np.ones((4,2))
|
||||
X_center2[0:3,0] = [0, 0, 0]
|
||||
X_center2[0:3,1] = [width, height, f_scale]
|
||||
|
||||
X_center3 = np.ones((4,2))
|
||||
X_center3[0:3,0] = [0, 0, 0]
|
||||
X_center3[0:3,1] = [width, -height, f_scale]
|
||||
|
||||
X_center4 = np.ones((4,2))
|
||||
X_center4[0:3,0] = [0, 0, 0]
|
||||
X_center4[0:3,1] = [-width, -height, f_scale]
|
||||
|
||||
# draw camera frame axis
|
||||
X_frame1 = np.ones((4,2))
|
||||
X_frame1[0:3,0] = [0, 0, 0]
|
||||
X_frame1[0:3,1] = [f_scale/2, 0, 0]
|
||||
|
||||
X_frame2 = np.ones((4,2))
|
||||
X_frame2[0:3,0] = [0, 0, 0]
|
||||
X_frame2[0:3,1] = [0, f_scale/2, 0]
|
||||
|
||||
X_frame3 = np.ones((4,2))
|
||||
X_frame3[0:3,0] = [0, 0, 0]
|
||||
X_frame3[0:3,1] = [0, 0, f_scale/2]
|
||||
|
||||
if draw_frame_axis:
|
||||
return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4, X_frame1, X_frame2, X_frame3]
|
||||
else:
|
||||
return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4]
|
||||
|
||||
def create_board_model(extrinsics, board_width, board_height, square_size, draw_frame_axis=False):
|
||||
width = board_width*square_size
|
||||
height = board_height*square_size
|
||||
|
||||
# draw calibration board
|
||||
X_board = np.ones((4,5))
|
||||
X_board_cam = np.ones((extrinsics.shape[0],4,5))
|
||||
X_board[0:3,0] = [0,0,0]
|
||||
X_board[0:3,1] = [width,0,0]
|
||||
X_board[0:3,2] = [width,height,0]
|
||||
X_board[0:3,3] = [0,height,0]
|
||||
X_board[0:3,4] = [0,0,0]
|
||||
|
||||
# draw board frame axis
|
||||
X_frame1 = np.ones((4,2))
|
||||
X_frame1[0:3,0] = [0, 0, 0]
|
||||
X_frame1[0:3,1] = [height/2, 0, 0]
|
||||
|
||||
X_frame2 = np.ones((4,2))
|
||||
X_frame2[0:3,0] = [0, 0, 0]
|
||||
X_frame2[0:3,1] = [0, height/2, 0]
|
||||
|
||||
X_frame3 = np.ones((4,2))
|
||||
X_frame3[0:3,0] = [0, 0, 0]
|
||||
X_frame3[0:3,1] = [0, 0, height/2]
|
||||
|
||||
if draw_frame_axis:
|
||||
return [X_board, X_frame1, X_frame2, X_frame3]
|
||||
else:
|
||||
return [X_board]
|
||||
|
||||
def draw_camera_boards(ax, camera_matrix, cam_width, cam_height, scale_focal,
|
||||
extrinsics, board_width, board_height, square_size,
|
||||
patternCentric):
|
||||
min_values = np.zeros((3,1))
|
||||
min_values = np.inf
|
||||
max_values = np.zeros((3,1))
|
||||
max_values = -np.inf
|
||||
|
||||
if patternCentric:
|
||||
X_moving = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal)
|
||||
X_static = create_board_model(extrinsics, board_width, board_height, square_size)
|
||||
else:
|
||||
X_static = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal, True)
|
||||
X_moving = create_board_model(extrinsics, board_width, board_height, square_size)
|
||||
|
||||
cm_subsection = linspace(0.0, 1.0, extrinsics.shape[0])
|
||||
colors = [ cm.jet(x) for x in cm_subsection ]
|
||||
|
||||
for i in range(len(X_static)):
|
||||
X = np.zeros(X_static[i].shape)
|
||||
for j in range(X_static[i].shape[1]):
|
||||
X[:,j] = transform_to_matplotlib_frame(np.eye(4), X_static[i][:,j])
|
||||
ax.plot3D(X[0,:], X[1,:], X[2,:], color='r')
|
||||
min_values = np.minimum(min_values, X[0:3,:].min(1))
|
||||
max_values = np.maximum(max_values, X[0:3,:].max(1))
|
||||
|
||||
for idx in range(extrinsics.shape[0]):
|
||||
R, _ = cv.Rodrigues(extrinsics[idx,0:3])
|
||||
cMo = np.eye(4,4)
|
||||
cMo[0:3,0:3] = R
|
||||
cMo[0:3,3] = extrinsics[idx,3:6]
|
||||
for i in range(len(X_moving)):
|
||||
X = np.zeros(X_moving[i].shape)
|
||||
for j in range(X_moving[i].shape[1]):
|
||||
X[0:4,j] = transform_to_matplotlib_frame(cMo, X_moving[i][0:4,j], patternCentric)
|
||||
ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[idx])
|
||||
min_values = np.minimum(min_values, X[0:3,:].min(1))
|
||||
max_values = np.maximum(max_values, X[0:3,:].max(1))
|
||||
|
||||
return min_values, max_values
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Plot camera calibration extrinsics.',
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument('--calibration', type=str, default="../data/left_intrinsics.yml",
|
||||
help='YAML camera calibration file.')
|
||||
parser.add_argument('--cam_width', type=float, default=0.064/2,
|
||||
help='Width/2 of the displayed camera.')
|
||||
parser.add_argument('--cam_height', type=float, default=0.048/2,
|
||||
help='Height/2 of the displayed camera.')
|
||||
parser.add_argument('--scale_focal', type=float, default=40,
|
||||
help='Value to scale the focal length.')
|
||||
parser.add_argument('--patternCentric', action='store_true',
|
||||
help='The calibration board is static and the camera is moving.')
|
||||
args = parser.parse_args()
|
||||
|
||||
fs = cv.FileStorage(args.calibration, cv.FILE_STORAGE_READ)
|
||||
board_width = int(fs.getNode('board_width').real())
|
||||
board_height = int(fs.getNode('board_height').real())
|
||||
square_size = fs.getNode('square_size').real()
|
||||
camera_matrix = fs.getNode('camera_matrix').mat()
|
||||
extrinsics = fs.getNode('extrinsic_parameters').mat()
|
||||
|
||||
fig = plt.figure()
|
||||
ax = fig.gca(projection='3d')
|
||||
ax.set_aspect("equal")
|
||||
|
||||
cam_width = args.cam_width
|
||||
cam_height = args.cam_height
|
||||
scale_focal = args.scale_focal
|
||||
min_values, max_values = draw_camera_boards(ax, camera_matrix, cam_width, cam_height,
|
||||
scale_focal, extrinsics, board_width,
|
||||
board_height, square_size, args.patternCentric)
|
||||
|
||||
X_min = min_values[0]
|
||||
X_max = max_values[0]
|
||||
Y_min = min_values[1]
|
||||
Y_max = max_values[1]
|
||||
Z_min = min_values[2]
|
||||
Z_max = max_values[2]
|
||||
max_range = np.array([X_max-X_min, Y_max-Y_min, Z_max-Z_min]).max() / 2.0
|
||||
|
||||
mid_x = (X_max+X_min) * 0.5
|
||||
mid_y = (Y_max+Y_min) * 0.5
|
||||
mid_z = (Z_max+Z_min) * 0.5
|
||||
ax.set_xlim(mid_x - max_range, mid_x + max_range)
|
||||
ax.set_ylim(mid_y - max_range, mid_y + max_range)
|
||||
ax.set_zlim(mid_z - max_range, mid_z + max_range)
|
||||
|
||||
ax.set_xlabel('x')
|
||||
ax.set_ylabel('z')
|
||||
ax.set_zlabel('-y')
|
||||
ax.set_title('Extrinsic Parameters Visualization')
|
||||
|
||||
plt.show()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Loading…
Reference in New Issue
Block a user