diff --git a/samples/data/left_intrinsics.yml b/samples/data/left_intrinsics.yml index 36ca56cab2..a90a02e9e3 100644 --- a/samples/data/left_intrinsics.yml +++ b/samples/data/left_intrinsics.yml @@ -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 ] diff --git a/samples/python/camera_calibration_show_extrinsics.py b/samples/python/camera_calibration_show_extrinsics.py new file mode 100755 index 0000000000..7b1b0cf980 --- /dev/null +++ b/samples/python/camera_calibration_show_extrinsics.py @@ -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()