mirror of https://github.com/opencv/opencv.git
Update left_intrinsics.yml file with chessboard square_size=0.025 and with extrinsic parameters. Add camera_calibration_show_extrinsics.py, a Python sample that allows to display the extrinsics saved during the camera calibration process (cpp-example-calibration).
catree
7 years ago
parent
dcdd6af5a8
commit
59ec224009
2 changed files with 275 additions and 7 deletions
@ -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 ] |
||||
|
||||
@ -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