Merge pull request #10354 from catree:add_python_sample_show_extrinsics

samples/data/left_intrinsics.yml

 %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 ]

samples/python/camera_calibration_show_extrinsics.py

+#!/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()