From 55ef393970c2ccbcca72bcf5179cc8a3f8cd0815 Mon Sep 17 00:00:00 2001 From: Matt Clarkson Date: Fri, 12 Apr 2024 20:09:32 +0100 Subject: [PATCH] Issue #62: Fix indexing to reduce warnings on numpy > 1.25 --- .../algorithms/triangulate.py | 72 +++--- .../video/video_calibration_cost_functions.py | 72 +++--- .../video/video_calibration_wrapper.py | 240 +++++++++--------- 3 files changed, 194 insertions(+), 190 deletions(-) diff --git a/sksurgerycalibration/algorithms/triangulate.py b/sksurgerycalibration/algorithms/triangulate.py index 3d97724..46ddd30 100644 --- a/sksurgerycalibration/algorithms/triangulate.py +++ b/sksurgerycalibration/algorithms/triangulate.py @@ -33,26 +33,25 @@ def _triangulate_point_using_svd(p1_array, # Build matrix A for homogeneous equation system Ax = 0 # Assume X = (x,y,z,1), for Linear-LS method # Which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1 - a_array = np.zeros((4, 3), dtype=np.double) - a_array[0, 0] = (u1_array[0] * p1_array[2, 0] - p1_array[0, 0]) / w1_const - a_array[0, 1] = (u1_array[0] * p1_array[2, 1] - p1_array[0, 1]) / w1_const - a_array[0, 2] = (u1_array[0] * p1_array[2, 2] - p1_array[0, 2]) / w1_const - a_array[1, 0] = (u1_array[1] * p1_array[2, 0] - p1_array[1, 0]) / w1_const - a_array[1, 1] = (u1_array[1] * p1_array[2, 1] - p1_array[1, 1]) / w1_const - a_array[1, 2] = (u1_array[1] * p1_array[2, 2] - p1_array[1, 2]) / w1_const - a_array[2, 0] = (u2_array[0] * p2_array[2, 0] - p2_array[0, 0]) / w2_const - a_array[2, 1] = (u2_array[0] * p2_array[2, 1] - p2_array[0, 1]) / w2_const - a_array[2, 2] = (u2_array[0] * p2_array[2, 2] - p2_array[0, 2]) / w2_const - a_array[3, 0] = (u2_array[1] * p2_array[2, 0] - p2_array[1, 0]) / w2_const - a_array[3, 1] = (u2_array[1] * p2_array[2, 1] - p2_array[1, 1]) / w2_const - a_array[3, 2] = (u2_array[1] * p2_array[2, 2] - p2_array[1, 2]) / w2_const + a_array[0, 0] = (u1_array[0][0] * p1_array[2, 0] - p1_array[0, 0]) / w1_const + a_array[0, 1] = (u1_array[0][0] * p1_array[2, 1] - p1_array[0, 1]) / w1_const + a_array[0, 2] = (u1_array[0][0] * p1_array[2, 2] - p1_array[0, 2]) / w1_const + a_array[1, 0] = (u1_array[1][0] * p1_array[2, 0] - p1_array[1, 0]) / w1_const + a_array[1, 1] = (u1_array[1][0] * p1_array[2, 1] - p1_array[1, 1]) / w1_const + a_array[1, 2] = (u1_array[1][0] * p1_array[2, 2] - p1_array[1, 2]) / w1_const + a_array[2, 0] = (u2_array[0][0] * p2_array[2, 0] - p2_array[0, 0]) / w2_const + a_array[2, 1] = (u2_array[0][0] * p2_array[2, 1] - p2_array[0, 1]) / w2_const + a_array[2, 2] = (u2_array[0][0] * p2_array[2, 2] - p2_array[0, 2]) / w2_const + a_array[3, 0] = (u2_array[1][0] * p2_array[2, 0] - p2_array[1, 0]) / w2_const + a_array[3, 1] = (u2_array[1][0] * p2_array[2, 1] - p2_array[1, 1]) / w2_const + a_array[3, 2] = (u2_array[1][0] * p2_array[2, 2] - p2_array[1, 2]) / w2_const b_array = np.zeros((4, 1), dtype=np.double) - b_array[0] = -(u1_array[0] * p1_array[2, 3] - p1_array[0, 3]) / w1_const - b_array[1] = -(u1_array[1] * p1_array[2, 3] - p1_array[1, 3]) / w1_const - b_array[2] = -(u2_array[0] * p2_array[2, 3] - p2_array[0, 3]) / w2_const - b_array[3] = -(u2_array[1] * p2_array[2, 3] - p2_array[1, 3]) / w2_const + b_array[0][0] = -(u1_array[0][0] * p1_array[2, 3] - p1_array[0, 3]) / w1_const + b_array[1][0] = -(u1_array[1][0] * p1_array[2, 3] - p1_array[1, 3]) / w1_const + b_array[2][0] = -(u2_array[0][0] * p2_array[2, 3] - p2_array[0, 3]) / w2_const + b_array[3][0] = -(u2_array[1][0] * p2_array[2, 3] - p2_array[1, 3]) / w2_const # start = time.time_ns() x_array = cv2.solve(a_array, b_array, flags=cv2.DECOMP_SVD) @@ -89,10 +88,10 @@ def _iter_triangulate_point_w_svd(p1_array, # Hartley suggests 10 iterations at most for dummy_idx in range(10): x_array_ = _triangulate_point_using_svd(p1_array, p2_array, u1_array, u2_array, w1_const, w2_const) - x_array[0] = x_array_[0] - x_array[1] = x_array_[1] - x_array[2] = x_array_[2] - x_array[3] = 1.0 + x_array[0][0] = x_array_[0][0] + x_array[1][0] = x_array_[1][0] + x_array[2][0] = x_array_[2][0] + x_array[3][0] = 1.0 p2x1 = p1_array[2, :].dot(x_array) p2x2 = p2_array[2, :].dot(x_array) @@ -100,12 +99,12 @@ def _iter_triangulate_point_w_svd(p1_array, if (abs(w1_const - p2x1) <= epsilon and abs(w2_const - p2x2) <= epsilon): break - w1_const = p2x1 - w2_const = p2x2 + w1_const = p2x1[0] + w2_const = p2x2[0] - result[0] = x_array[0] - result[1] = x_array[1] - result[2] = x_array[2] + result[0][0] = x_array[0][0] + result[1][0] = x_array[1][0] + result[2][0] = x_array[2][0] return result @@ -122,10 +121,15 @@ def new_e2_array(e2_array, r2_array, left_to_right_trans_vector): :return e2_array: [4x4] narray """ + # While the specification is [3x1] which implies ndarray, the users + # may also pass in array (3,), ndarray (3, 1) or ndarray (1, 3). + # So, this will flatten all of them to the same array-like shape. + l_r = np.ravel(left_to_right_trans_vector) + for row_idx in range(0, 3): for col_idx in range(0, 3): e2_array[row_idx, col_idx] = r2_array[row_idx, col_idx] - e2_array[row_idx, 3] = left_to_right_trans_vector[row_idx] + e2_array[row_idx, 3] = l_r[row_idx] return e2_array @@ -231,9 +235,9 @@ def triangulate_points_hartley(input_undistorted_points, # array shapes for input args _iter_triangulate_point_w_svd( [3, 4]; [3, 4]; [3, 1]; [3, 1] ) reconstructed_point = _iter_triangulate_point_w_svd(p1d, p2d, u1t, u2t) - output_points[dummy_index, 0] = reconstructed_point[0] - output_points[dummy_index, 1] = reconstructed_point[1] - output_points[dummy_index, 2] = reconstructed_point[2] + output_points[dummy_index, 0] = reconstructed_point[0][0] + output_points[dummy_index, 1] = reconstructed_point[1][0] + output_points[dummy_index, 2] = reconstructed_point[2][0] return output_points @@ -301,10 +305,10 @@ def triangulate_points_opencv(input_undistorted_points, # array shapes for input args cv2.triangulatePoints( [3, 4]; [3, 4]; [2, 1]; [2, 1] ) reconstructed_point = cv2.triangulatePoints(p1mat, p2mat, u1t[:2], u2t[:2]) - reconstructed_point /= reconstructed_point[3] # Homogenize + reconstructed_point /= reconstructed_point[3][0] # Homogenize - output_points[dummy_index, 0] = reconstructed_point[0] - output_points[dummy_index, 1] = reconstructed_point[1] - output_points[dummy_index, 2] = reconstructed_point[2] + output_points[dummy_index, 0] = reconstructed_point[0][0] + output_points[dummy_index, 1] = reconstructed_point[1][0] + output_points[dummy_index, 2] = reconstructed_point[2][0] return output_points diff --git a/sksurgerycalibration/video/video_calibration_cost_functions.py b/sksurgerycalibration/video/video_calibration_cost_functions.py index 899a1b7..d480e18 100644 --- a/sksurgerycalibration/video/video_calibration_cost_functions.py +++ b/sksurgerycalibration/video/video_calibration_cost_functions.py @@ -77,14 +77,14 @@ def mono_proj_err_h2e(x_0, assert len(x_0) == 6 rvec = np.zeros((3, 1)) - rvec[0] = x_0[0] - rvec[1] = x_0[1] - rvec[2] = x_0[2] + rvec[0][0] = x_0[0] + rvec[1][0] = x_0[1] + rvec[2][0] = x_0[2] tvec = np.zeros((3, 1)) - tvec[0] = x_0[3] - tvec[1] = x_0[4] - tvec[2] = x_0[5] + tvec[0][0] = x_0[3] + tvec[1][0] = x_0[4] + tvec[2][0] = x_0[5] h2e = vu.extrinsic_vecs_to_matrix(rvec, tvec) @@ -131,24 +131,24 @@ def mono_proj_err_p2m_h2e(x_0, assert len(x_0) == 12 rvec = np.zeros((3, 1)) - rvec[0] = x_0[0] - rvec[1] = x_0[1] - rvec[2] = x_0[2] + rvec[0][0] = x_0[0] + rvec[1][0] = x_0[1] + rvec[2][0] = x_0[2] tvec = np.zeros((3, 1)) - tvec[0] = x_0[3] - tvec[1] = x_0[4] - tvec[2] = x_0[5] + tvec[0][0] = x_0[3] + tvec[1][0] = x_0[4] + tvec[2][0] = x_0[5] p2m = vu.extrinsic_vecs_to_matrix(rvec, tvec) - rvec[0] = x_0[6] - rvec[1] = x_0[7] - rvec[2] = x_0[8] + rvec[0][0] = x_0[6] + rvec[1][0] = x_0[7] + rvec[2][0] = x_0[8] - tvec[0] = x_0[9] - tvec[1] = x_0[10] - tvec[2] = x_0[11] + tvec[0][0] = x_0[9] + tvec[1][0] = x_0[10] + tvec[2][0] = x_0[11] h2e = vu.extrinsic_vecs_to_matrix(rvec, tvec) @@ -194,24 +194,24 @@ def mono_proj_err_h2e_g2w(x_0, assert len(x_0) == 12 rvec = np.zeros((3, 1)) - rvec[0] = x_0[0] - rvec[1] = x_0[1] - rvec[2] = x_0[2] + rvec[0][0] = x_0[0] + rvec[1][0] = x_0[1] + rvec[2][0] = x_0[2] tvec = np.zeros((3, 1)) - tvec[0] = x_0[3] - tvec[1] = x_0[4] - tvec[2] = x_0[5] + tvec[0][0] = x_0[3] + tvec[1][0] = x_0[4] + tvec[2][0] = x_0[5] h2e = vu.extrinsic_vecs_to_matrix(rvec, tvec) - rvec[0] = x_0[6] - rvec[1] = x_0[7] - rvec[2] = x_0[8] + rvec[0][0] = x_0[6] + rvec[1][0] = x_0[7] + rvec[2][0] = x_0[8] - tvec[0] = x_0[9] - tvec[1] = x_0[10] - tvec[2] = x_0[11] + tvec[0][0] = x_0[9] + tvec[1][0] = x_0[10] + tvec[2][0] = x_0[11] g2w = vu.extrinsic_vecs_to_matrix(rvec, tvec) @@ -256,14 +256,14 @@ def mono_proj_err_h2e_int_dist(x_0, assert len(x_0) == 15 rvec = np.zeros((3, 1)) - rvec[0] = x_0[0] - rvec[1] = x_0[1] - rvec[2] = x_0[2] + rvec[0][0] = x_0[0] + rvec[1][0] = x_0[1] + rvec[2][0] = x_0[2] tvec = np.zeros((3, 1)) - tvec[0] = x_0[3] - tvec[1] = x_0[4] - tvec[2] = x_0[5] + tvec[0][0] = x_0[3] + tvec[1][0] = x_0[4] + tvec[2][0] = x_0[5] h2e = vu.extrinsic_vecs_to_matrix(rvec, tvec) diff --git a/sksurgerycalibration/video/video_calibration_wrapper.py b/sksurgerycalibration/video/video_calibration_wrapper.py index 96901cf..d135c9b 100644 --- a/sksurgerycalibration/video/video_calibration_wrapper.py +++ b/sksurgerycalibration/video/video_calibration_wrapper.py @@ -159,20 +159,20 @@ def mono_handeye_calibration(object_points: List, # Now optimise p2m and h2e x_0 = np.zeros(12) rvec, tvec = vu.extrinsic_matrix_to_vecs(pattern2marker_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] rvec, tvec = vu.extrinsic_matrix_to_vecs(handeye_matrix) - x_0[6] = rvec[0] - x_0[7] = rvec[1] - x_0[8] = rvec[2] - x_0[9] = tvec[0] - x_0[10] = tvec[1] - x_0[11] = tvec[2] + x_0[6] = rvec[0][0] + x_0[7] = rvec[1][0] + x_0[8] = rvec[2][0] + x_0[9] = tvec[0][0] + x_0[10] = tvec[1][0] + x_0[11] = tvec[2][0] res = minimize(vcf.mono_proj_err_p2m_h2e, x_0, args=(object_points, @@ -186,20 +186,20 @@ def mono_handeye_calibration(object_points: List, ) x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] pattern2marker_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) - rvec[0] = x_1[6] - rvec[1] = x_1[7] - rvec[2] = x_1[8] - tvec[0] = x_1[9] - tvec[1] = x_1[10] - tvec[2] = x_1[11] + rvec[0][0] = x_1[6] + rvec[1][0] = x_1[7] + rvec[2][0] = x_1[8] + tvec[0][0] = x_1[9] + tvec[1][0] = x_1[10] + tvec[2][0] = x_1[11] handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) elif pattern2marker_matrix is not None \ @@ -218,12 +218,12 @@ def mono_handeye_calibration(object_points: List, # Now optimise just the h2e x_0 = np.zeros(6) rvec, tvec = vu.extrinsic_matrix_to_vecs(handeye_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] res = minimize(vcf.mono_proj_err_h2e, x_0, args=(object_points, @@ -238,12 +238,12 @@ def mono_handeye_calibration(object_points: List, ) x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) else: @@ -262,12 +262,12 @@ def mono_handeye_calibration(object_points: List, # Now optimise h2e, intrinsics, distortion x_0 = np.zeros(15) rvec, tvec = vu.extrinsic_matrix_to_vecs(handeye_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] x_0[6] = camera_matrix[0][0] x_0[7] = camera_matrix[1][1] x_0[8] = camera_matrix[0][2] @@ -288,12 +288,12 @@ def mono_handeye_calibration(object_points: List, method='Powell', ) x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) camera_matrix[0][0] = x_1[6] @@ -703,20 +703,20 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, x_0 = np.zeros(12) rvec, tvec = vu.extrinsic_matrix_to_vecs(left_handeye_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] rvec, tvec = vu.extrinsic_matrix_to_vecs(left_pattern2marker_matrix) - x_0[6] = rvec[0] - x_0[7] = rvec[1] - x_0[8] = rvec[2] - x_0[9] = tvec[0] - x_0[10] = tvec[1] - x_0[11] = tvec[2] + x_0[6] = rvec[0][0] + x_0[7] = rvec[1][0] + x_0[8] = rvec[2][0] + x_0[9] = tvec[0][0] + x_0[10] = tvec[1][0] + x_0[11] = tvec[2][0] res = minimize(vcf.stereo_proj_err_h2e, x_0, args=(common_object_pts, @@ -743,20 +743,20 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] left_handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) - rvec[0] = x_1[6] - rvec[1] = x_1[7] - rvec[2] = x_1[8] - tvec[0] = x_1[9] - tvec[1] = x_1[10] - tvec[2] = x_1[11] + rvec[0][0] = x_1[6] + rvec[1][0] = x_1[7] + rvec[2][0] = x_1[8] + tvec[0][0] = x_1[9] + tvec[1][0] = x_1[10] + tvec[2][0] = x_1[11] left_pattern2marker_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) elif override_pattern2marker is not None \ @@ -766,12 +766,12 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, # Now optimise just the h2e x_0 = np.zeros(6) rvec, tvec = vu.extrinsic_matrix_to_vecs(left_handeye_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] res = minimize(vcf.stereo_proj_err_h2e, x_0, args=(common_object_pts, @@ -797,12 +797,12 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, str(res.message)) x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] left_handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) # Now, final case, optimise handeye and stereo camera parameters. @@ -813,21 +813,21 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, x_0 = np.zeros(30) rvec, tvec = vu.extrinsic_matrix_to_vecs(left_handeye_matrix) - x_0[0] = rvec[0] - x_0[1] = rvec[1] - x_0[2] = rvec[2] - x_0[3] = tvec[0] - x_0[4] = tvec[1] - x_0[5] = tvec[2] + x_0[0] = rvec[0][0] + x_0[1] = rvec[1][0] + x_0[2] = rvec[2][0] + x_0[3] = tvec[0][0] + x_0[4] = tvec[1][0] + x_0[5] = tvec[2][0] l2r = skcm.construct_rigid_transformation(l2r_rmat, l2r_tvec) rvec, tvec = vu.extrinsic_matrix_to_vecs(l2r) - x_0[6] = rvec[0] - x_0[7] = rvec[1] - x_0[8] = rvec[2] - x_0[9] = tvec[0] - x_0[10] = tvec[1] - x_0[11] = tvec[2] + x_0[6] = rvec[0][0] + x_0[7] = rvec[1][0] + x_0[8] = rvec[2][0] + x_0[9] = tvec[0][0] + x_0[10] = tvec[1][0] + x_0[11] = tvec[2][0] x_0[12] = left_camera_matrix[0][0] x_0[13] = left_camera_matrix[1][1] @@ -867,20 +867,20 @@ def stereo_handeye_calibration(l2r_rmat: np.ndarray, str(res.message)) x_1 = res.x - rvec[0] = x_1[0] - rvec[1] = x_1[1] - rvec[2] = x_1[2] - tvec[0] = x_1[3] - tvec[1] = x_1[4] - tvec[2] = x_1[5] + rvec[0][0] = x_1[0] + rvec[1][0] = x_1[1] + rvec[2][0] = x_1[2] + tvec[0][0] = x_1[3] + tvec[1][0] = x_1[4] + tvec[2][0] = x_1[5] left_handeye_matrix = vu.extrinsic_vecs_to_matrix(rvec, tvec) - rvec[0] = x_1[6] - rvec[1] = x_1[7] - rvec[2] = x_1[8] - tvec[0] = x_1[9] - tvec[1] = x_1[10] - tvec[2] = x_1[11] + rvec[0][0] = x_1[6] + rvec[1][0] = x_1[7] + rvec[2][0] = x_1[8] + tvec[0][0] = x_1[9] + tvec[1][0] = x_1[10] + tvec[2][0] = x_1[11] l2r = vu.extrinsic_vecs_to_matrix(rvec, tvec) l2r_rmat = l2r[0:3, 0:3] l2r_tvec = l2r[0:3, 3] @@ -1008,12 +1008,12 @@ def stereo_calibration_extrinsics(common_object_points, number_of_parameters = 6 * number_of_frames x_0 = np.zeros(number_of_parameters) for i in range(0, number_of_frames): - x_0[i * 6 + 0] = l_rvecs[i][0] - x_0[i * 6 + 1] = l_rvecs[i][1] - x_0[i * 6 + 2] = l_rvecs[i][2] - x_0[i * 6 + 3] = l_tvecs[i][0] - x_0[i * 6 + 4] = l_tvecs[i][1] - x_0[i * 6 + 5] = l_tvecs[i][2] + x_0[i * 6 + 0] = l_rvecs[i][0][0] + x_0[i * 6 + 1] = l_rvecs[i][1][0] + x_0[i * 6 + 2] = l_rvecs[i][2][0] + x_0[i * 6 + 3] = l_tvecs[i][0][0] + x_0[i * 6 + 4] = l_tvecs[i][1][0] + x_0[i * 6 + 5] = l_tvecs[i][2][0] res = least_squares(vcf.stereo_2d_error_for_extrinsics, x_0, args=(common_object_points, @@ -1035,11 +1035,11 @@ def stereo_calibration_extrinsics(common_object_points, x_1 = res.x for i in range(0, number_of_frames): - l_rvecs[i][0] = x_1[i * 6 + 0] - l_rvecs[i][1] = x_1[i * 6 + 1] - l_rvecs[i][2] = x_1[i * 6 + 2] - l_tvecs[i][0] = x_1[i * 6 + 3] - l_tvecs[i][1] = x_1[i * 6 + 4] - l_tvecs[i][2] = x_1[i * 6 + 5] + l_rvecs[i][0][0] = x_1[i * 6 + 0] + l_rvecs[i][1][0] = x_1[i * 6 + 1] + l_rvecs[i][2][0] = x_1[i * 6 + 2] + l_tvecs[i][0][0] = x_1[i * 6 + 3] + l_tvecs[i][1][0] = x_1[i * 6 + 4] + l_tvecs[i][2][0] = x_1[i * 6 + 5] return res.fun, l_rvecs, l_tvecs