Download samples/python/camera_calibration_show_extrinsics.py from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 8.14 kB
-
https://huggingface.co/tgollogly/test/resolve/main/samples/python/camera_calibration_show_extrinsics.py
- Command line
-
hf download hf://tgollogly/test/samples/python/camera_calibration_show_extrinsics.py
-
curl -L -o camera_calibration_show_extrinsics.py https://huggingface.co/tgollogly/test/resolve/main/samples/python/camera_calibration_show_extrinsics.py
8.14 kB
| #!/usr/bin/env python | |
| ''' | |
| Plot camera calibration extrinsics. | |
| usage: | |
| camera_calibration_show_extrinsics.py [--calibration <input path>] [--cam_width] [--cam_height] [--scale_focal] [--patternCentric ] | |
| default values: | |
| --calibration : left_intrinsics.yml | |
| --cam_width : 0.064/2 | |
| --cam_height : 0.048/2 | |
| --scale_focal : 40 | |
| --patternCentric : True | |
| ''' | |
| # Python 2/3 compatibility | |
| from __future__ import print_function | |
| import numpy as np | |
| import cv2 as cv | |
| from numpy import linspace | |
| 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): | |
| from matplotlib import cm | |
| 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(): | |
| import argparse | |
| parser = argparse.ArgumentParser(description='Plot camera calibration extrinsics.', | |
| formatter_class=argparse.ArgumentDefaultsHelpFormatter) | |
| parser.add_argument('--calibration', type=str, default='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(cv.samples.findFile(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() | |
| import matplotlib.pyplot as plt | |
| from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-variable | |
| fig = plt.figure() | |
| ax = fig.gca(projection='3d') | |
| ax.set_aspect("auto") | |
| 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() | |
| print('Done') | |
| if __name__ == '__main__': | |
| print(__doc__) | |
| main() | |
| cv.destroyAllWindows() | |