def save_data(self, dir_name: str, file_prefix: str): """ Saves calibration parameters to a directory. :param dir_name: directory to save to :param file_prefix: prefix for all files """ if not os.path.isdir(dir_name): os.makedirs(dir_name) intrinsics_file = sksio.get_intrinsics_file_name(dir_name, file_prefix) np.savetxt(intrinsics_file, self.camera_matrix, fmt='%.8f') dist_coeff_file = sksio.get_distortion_file_name(dir_name, file_prefix) np.savetxt(dist_coeff_file, self.dist_coeffs, fmt='%.8f') handeye_file = sksio.get_handeye_file_name(dir_name, file_prefix) np.savetxt(handeye_file, self.handeye_matrix, fmt='%.8f') p2m_file = sksio.get_pattern2marker_file_name(dir_name, file_prefix) np.savetxt(p2m_file, self.pattern2marker_matrix, fmt='%.8f') for i in enumerate(self.rvecs): extrinsics_file = sksio.get_extrinsics_file_name( dir_name, file_prefix, i[0]) extrinsics = sksu.extrinsic_vecs_to_matrix(self.rvecs[i[0]], self.tvecs[i[0]]) np.savetxt(extrinsics_file, extrinsics, fmt='%.8f')
def compute_mono_3d_err(ids, object_points, image_points, rvecs, tvecs, camera_matrix, distortion): """ Function to compute mono RMS reconstruction error over multiple views. Here, to triangulate, we take the i^th camera as left camera, and the i+1^th camera as the right camera, compute l2r, and triangulate. :param ids: Vector of ndarray of integer point ids :param object_points: Vector of Vector of 1x3 of type float32 :param image_points: Vector of Vector of 1x2 of type float32 :param rvecs: Vector of [3x1] ndarray, Rodrigues rotations for each camera :param tvecs: Vector of [3x1] ndarray, translations for each camera :param camera_matrix: [3x3] ndarray :param distortion: [1x5] ndarray :return: SSE re-reprojection error, number_samples """ sse = 0 number_of_samples = 0 number_of_frames = len(object_points) # We are going to triangulate between a frame and the next frame. for i in range(0, number_of_frames): j = i + 1 if j == number_of_frames: j = 0 _, common_object_points, common_left_image_points, \ common_right_image_points = \ vu.filter_common_points_per_image(ids[i], object_points[i], image_points[i], ids[j], image_points[j], 10) left_camera_to_world = vu.extrinsic_vecs_to_matrix(rvecs[i], tvecs[i]) right_camera_to_world = vu.extrinsic_vecs_to_matrix(rvecs[j], tvecs[j]) left_to_right = np.matmul(right_camera_to_world, np.linalg.inv(left_camera_to_world)) l2r_rmat = left_to_right[0:3, 0:3] l2r_tvec = left_to_right[0:3, 3] c_obj = [common_object_points] c_li = [common_left_image_points] c_ri = [common_right_image_points] r_v = [rvecs[i]] t_v = [tvecs[i]] err, samp = compute_stereo_3d_error(l2r_rmat, l2r_tvec, c_obj, c_li, camera_matrix, distortion, c_ri, camera_matrix, distortion, r_v, t_v) sse = sse + err number_of_samples = number_of_samples + samp LOGGER.debug("Mono RMS reconstruction: sse=%s, num=%s", str(sse), str(number_of_samples)) return sse, number_of_samples
def compute_stereo_2d_err(l2r_rmat, l2r_tvec, left_object_points, left_image_points, left_camera_matrix, left_distortion, right_object_points, right_image_points, right_camera_matrix, right_distortion, left_rvecs, left_tvecs, return_residuals=False): """ Function to compute stereo re-projection error, over multiple views. :param l2r_rmat: [3x3] ndarray, rotation for l2r transform :param l2r_tvec: [3x1] ndarray, translation for l2r transform :param left_object_points: Vector of Vector of 1x3 of type float32 :param left_image_points: Vector of Vector of 1x2 of type float32 :param left_camera_matrix: [3x3] ndarray :param left_distortion: [1x5] ndarray :param right_object_points: Vector of Vector of 1x3 of type float32 :param right_image_points: Vector of Vector of 1x2 of type float32 :param right_camera_matrix: [3x3] ndarray :param right_distortion: [1x5] ndarray :param left_rvecs: Vector of [3x1] ndarray, Rodrigues rotations, left camera :param left_tvecs: Vector of [3x1] ndarray, translations, left camera :param return_residuals: if True returns vector of residuals for LM, otherwise, returns SSE. :return: re-reprojection error, number_samples """ left_to_right = mu.construct_rigid_transformation(l2r_rmat, l2r_tvec) lse = 0 rse = 0 residuals = [] number_of_samples = 0 number_of_frames = len(left_object_points) for i in range(0, number_of_frames): projected_left, _ = cv2.projectPoints(left_object_points[i], left_rvecs[i], left_tvecs[i], left_camera_matrix, left_distortion) world_to_left_camera = vu.extrinsic_vecs_to_matrix( left_rvecs[i], left_tvecs[i]) world_to_right_camera = np.matmul(left_to_right, world_to_left_camera) rvec, tvec = vu.extrinsic_matrix_to_vecs(world_to_right_camera) projected_right, _ = cv2.projectPoints(right_object_points[i], rvec, tvec, right_camera_matrix, right_distortion) number_of_samples = number_of_samples \ + len(left_image_points[i]) \ + len(right_image_points[i]) diff_left = left_image_points[i] - projected_left diff_right = right_image_points[i] - projected_right if return_residuals: residuals.append(diff_left.reshape((-1))) residuals.append(diff_right.reshape((-1))) else: lse = lse + np.sum(np.square(diff_left)) rse = rse + np.sum(np.square(diff_right)) if return_residuals: return np.hstack(residuals) return lse + rse, number_of_samples
def stereo_video_calibration_expt(left_ids, left_object_points, left_image_points, right_ids, right_object_points, right_image_points, image_size): """ Experimental. :param left_ids: Vector of ndarrays containing integer point ids. :param left_object_points: Vector of Vectors of 1x3 object points, float32 :param left_image_points: Vector of Vectors of 1x2 object points, float32 :param right_ids: Vector of ndarrays containing integer point ids. :param right_object_points: Vector of Vectors of 1x3 object points, float32 :param right_image_points: Vector of Vectors of 1x2 object points, float32 :param image_size: (x, y) tuple, size in pixels, e.g. (1920, 1080) :return: """ # First do standard OpenCV calibration, using the wrapper in this project. s_reproj, s_recon, \ l_c, l_d, l_rvecs, l_tvecs, \ r_c, r_d, r_rvecs, r_tvecs, \ l2r_r, l2r_t, \ essential, fundamental = \ vc.stereo_video_calibration(left_ids, left_object_points, left_image_points, right_ids, right_object_points, right_image_points, image_size) # For stereo reconstruction error we need common points. _, common_object_points, common_left_image_points, \ common_right_image_points \ = vu.filter_common_points_all_images(left_ids, left_object_points, left_image_points, right_ids, right_image_points, 10) l2r_rvec = (cv2.Rodrigues(l2r_r.T))[0] # Now compute a set of left extrinsics, where the parameters # are all relative to the first camera. camera_rvecs = copy.deepcopy(l_rvecs) camera_tvecs = copy.deepcopy(l_tvecs) number_of_frames = len(left_object_points) first_world_to_camera = \ vu.extrinsic_vecs_to_matrix(camera_rvecs[0], camera_tvecs[0]) first_camera_to_world = np.linalg.inv(first_world_to_camera) for i in range(1, number_of_frames): extrinsic = \ vu.extrinsic_vecs_to_matrix(camera_rvecs[i], camera_tvecs[i]) relative_to_first = np.matmul(extrinsic, first_camera_to_world) camera_rvecs[i], camera_tvecs[i] = \ vu.extrinsic_matrix_to_vecs(relative_to_first) # Now we have to create a flat vector of parameters to optimise. # Optimsing l2r_r (3), l2r_t(3), l_c (4), r_c (4) + 6 DOF per camera. number_of_parameters = 3 + 3 + 4 + 4 + (number_of_frames * 6) x_0 = np.zeros(number_of_parameters) x_0[0] = l2r_rvec[0][0] x_0[1] = l2r_rvec[1][0] x_0[2] = l2r_rvec[2][0] x_0[3] = l2r_t[0][0] x_0[4] = l2r_t[1][0] x_0[5] = l2r_t[2][0] x_0[6] = l_c[0][0] x_0[7] = l_c[1][1] x_0[8] = l_c[0][2] x_0[9] = l_c[1][2] x_0[10] = r_c[0][0] x_0[11] = r_c[1][1] x_0[12] = r_c[0][2] x_0[13] = r_c[1][2] for i in range(0, number_of_frames): x_0[14 + i * 6 + 0] = camera_rvecs[i][0] x_0[14 + i * 6 + 1] = camera_rvecs[i][1] x_0[14 + i * 6 + 2] = camera_rvecs[i][2] x_0[14 + i * 6 + 3] = camera_tvecs[i][0] x_0[14 + i * 6 + 4] = camera_tvecs[i][1] x_0[14 + i * 6 + 5] = camera_tvecs[i][2] res = minimize(vcf.stereo_2d_and_3d_error, x_0, args=(common_object_points, common_left_image_points, l_d, common_object_points, common_right_image_points, r_d), method='Powell', tol=1e-4, options={ 'disp': False, 'maxiter': 100000 }) LOGGER.info("Stereo Re-Calibration: success=%s", str(res.success)) LOGGER.info("Stereo Re-Calibration: msg=%s", str(res.message)) # Now need to unpack the results, into the same set of vectors, # as the stereo_video_calibration, so they are drop-in replacements. x_1 = res.x l2r_rvec[0][0] = x_1[0] l2r_rvec[1][0] = x_1[1] l2r_rvec[2][0] = x_1[2] l2r_r = (cv2.Rodrigues(l2r_rvec))[0] l2r_t[0][0] = x_1[3] l2r_t[1][0] = x_1[4] l2r_t[2][0] = x_1[5] l_c[0][0] = x_1[6] l_c[1][1] = x_1[7] l_c[0][2] = x_1[8] l_c[1][2] = x_1[9] r_c[0][0] = x_1[10] r_c[1][1] = x_1[11] r_c[0][2] = x_1[12] r_c[1][2] = x_1[13] for i in range(0, number_of_frames): camera_rvecs[i][0] = x_1[14 + i * 6 + 0] camera_rvecs[i][1] = x_1[14 + i * 6 + 1] camera_rvecs[i][2] = x_1[14 + i * 6 + 2] camera_tvecs[i][0] = x_1[14 + i * 6 + 3] camera_tvecs[i][1] = x_1[14 + i * 6 + 4] camera_tvecs[i][2] = x_1[14 + i * 6 + 5] # Still need to convert these parameters (which are relative to first # camera), into consistent world-to-camera rvecs and tvecs for left/right. first_world_to_camera = \ vu.extrinsic_vecs_to_matrix(camera_rvecs[0], camera_tvecs[0]) left_to_right = skcm.construct_rigid_transformation(l2r_r, l2r_t) for i in range(0, number_of_frames): if i == 0: left_world_to_camera = first_world_to_camera else: extrinsic = \ vu.extrinsic_vecs_to_matrix(camera_rvecs[i], camera_tvecs[i]) left_world_to_camera = np.matmul(extrinsic, first_world_to_camera) l_rvecs[i], l_tvecs[i] = \ vu.extrinsic_matrix_to_vecs(left_world_to_camera) right_world_to_camera = \ np.matmul(left_to_right, left_world_to_camera) r_rvecs[i], r_tvecs[i] = \ vu.extrinsic_matrix_to_vecs(right_world_to_camera) # And recompute stereo RMS re-projection error. s_reproj_2 = \ vm.compute_stereo_2d_err(l2r_r, l2r_t, common_object_points, common_left_image_points, l_c, l_d, common_object_points, common_right_image_points, r_c, r_d, l_rvecs, l_tvecs ) s_recon_2 = \ vm.compute_stereo_3d_error(l2r_r, l2r_t, common_object_points, common_left_image_points, l_c, l_d, common_right_image_points, r_c, r_d, l_rvecs, l_tvecs ) LOGGER.info( "Stereo Re-Calib: reproj_1=%s, recon_1=%s, reproj_2=%s, recon_2=%s", str(s_reproj), str(s_recon), str(s_reproj_2), str(s_recon_2)) return s_reproj_2, s_recon_2, \ l_c, l_d, l_rvecs, l_tvecs, \ r_c, r_d, r_rvecs, r_tvecs, \ l2r_r, l2r_t, \ essential, fundamental
def stereo_video_calibration(left_ids, left_object_points, left_image_points, right_ids, right_object_points, right_image_points, image_size, flags=cv2.CALIB_USE_INTRINSIC_GUESS, override_left_intrinsics=None, override_left_distortion=None, override_right_intrinsics=None, override_right_distortion=None, override_l2r_rmat=None, override_l2r_tvec=None): """ Default stereo calibration, using OpenCV methods. We wrap it here, so we have a place to add extra validation code, and a space for documentation. The aim is to check everything before we pass it to OpenCV, and raise Exceptions consistently for any error we can detect before we pass it to OpenCV. :param left_ids: Vector of ndarrays containing integer point ids. :param left_object_points: Vector of Vectors of 1x3 object points, float32 :param left_image_points: Vector of Vectors of 1x2 object points, float32 :param right_ids: Vector of ndarrays containing integer point ids. :param right_object_points: Vector of Vectors of 1x3 object points, float32 :param right_image_points: Vector of Vectors of 1x2 object points, float32 :param image_size: (x, y) tuple, size in pixels, e.g. (1920, 1080) :param flags: OpenCV flags to pass to calibrateCamera(). :return: """ # We only do override if all override params are specified. # pylint:disable=too-many-boolean-expressions do_override = False if override_left_intrinsics is not None \ and override_left_distortion is not None \ and override_right_intrinsics is not None \ and override_right_distortion is not None \ and override_l2r_rmat is not None \ and override_l2r_tvec is not None: do_override = True l_c = override_left_intrinsics l_d = override_left_distortion r_c = override_right_intrinsics r_d = override_right_distortion number_of_frames = len(left_object_points) l_rvecs = [] l_tvecs = [] r_rvecs = [] r_tvecs = [] if do_override: for i in range(0, number_of_frames): _, rvecs, tvecs = cv2.solvePnP(left_object_points[i], left_image_points[i], l_c, l_d) l_rvecs.append(rvecs) l_tvecs.append(tvecs) _, rvecs, tvecs = cv2.solvePnP(right_object_points[i], right_image_points[i], r_c, r_d) r_rvecs.append(rvecs) r_tvecs.append(tvecs) else: _, l_c, l_d, l_rvecs, l_tvecs \ = cv2.calibrateCamera(left_object_points, left_image_points, image_size, None, None) _, r_c, r_d, r_rvecs, r_tvecs \ = cv2.calibrateCamera(right_object_points, right_image_points, image_size, None, None) # For stereo, OpenCV needs common points. _, common_object_points, common_left_image_points, \ common_right_image_points \ = vu.filter_common_points_all_images(left_ids, left_object_points, left_image_points, right_ids, right_image_points, 10) if do_override: # Do OpenCV stereo calibration, using override intrinsics, # just so we can get the essential and fundamental matrix out. _, l_c, l_d, r_c, r_d, \ l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate( common_object_points, common_left_image_points, common_right_image_points, l_c, l_d, r_c, r_d, image_size, flags=cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_INTRINSIC) l2r_r = override_l2r_rmat l2r_t = override_l2r_tvec assert np.allclose(l_c, override_left_intrinsics) assert np.allclose(l_d, override_left_distortion) assert np.allclose(r_c, override_right_intrinsics) assert np.allclose(r_d, override_right_distortion) else: # Do OpenCV stereo calibration, using intrinsics from OpenCV mono. _, l_c, l_d, r_c, r_d, \ l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate( common_object_points, common_left_image_points, common_right_image_points, l_c, l_d, r_c, r_d, image_size, flags=cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_INTRINSIC) # Then do it again, using the passed in flags. _, l_c, l_d, r_c, r_d, \ l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate( common_object_points, common_left_image_points, common_right_image_points, l_c, l_d, r_c, r_d, image_size, flags=flags) if do_override: # Stereo calibration is hard for a laparoscope. # In clinical practice, the data may be way too variable. # For stereo scopes, they are often fixed focus, # i.e. fixed intrinsics, and fixed stereo. # So, we may prefer to just do the best possible calibration # in the lab, and then keep those values constant. # But we then would still want to optimise the camera extrinsics # as the camera poses directly affect the hand-eye calibration. _, l_rvecs, l_tvecs, \ = stereo_calibration_extrinsics( common_object_points, common_left_image_points, common_right_image_points, l_rvecs, l_tvecs, l_c, l_d, r_c, r_d, l2r_r, l2r_t ) else: # Normal OpenCV stereo calibration optimises intrinsics, # distortion, and stereo parameters, but doesn't output pose. # So here, we recompute the left camera pose. for i in range(0, number_of_frames): _, l_rvecs[i], l_tvecs[i] = cv2.solvePnP( common_object_points[i], common_left_image_points[i], l_c, l_d) # Here, we are computing the right hand side rvecs and tvecs # given the new left hand side rvecs, tvecs and the l2r. left_to_right = skcm.construct_rigid_transformation(l2r_r, l2r_t) for i in range(0, number_of_frames): left_chessboard_to_camera = \ vu.extrinsic_vecs_to_matrix(l_rvecs[i], l_tvecs[i]) right_chessboard_to_camera = \ np.matmul(left_to_right, left_chessboard_to_camera) r_rvecs[i], r_tvecs[i] = \ vu.extrinsic_matrix_to_vecs(right_chessboard_to_camera) # And recompute stereo projection error, given left camera and l2r. # We also use all points, not just common points, for comparison # with other methods outside of this function. sse, num_samples = \ vm.compute_stereo_2d_err(l2r_r, l2r_t, common_object_points, common_left_image_points, l_c, l_d, common_object_points, common_right_image_points, r_c, r_d, l_rvecs, l_tvecs ) mse = sse / num_samples s_reproj = np.sqrt(mse) sse, num_samples = \ vm.compute_stereo_3d_error(l2r_r, l2r_t, common_object_points, common_left_image_points, l_c, l_d, common_right_image_points, r_c, r_d, l_rvecs, l_tvecs ) mse = sse / num_samples s_recon = np.sqrt(mse) LOGGER.info("Stereo Calib: proj=%s, recon=%s", str(s_reproj), str(s_recon)) return s_reproj, s_recon, \ l_c, l_d, l_rvecs, l_tvecs, \ r_c, r_d, r_rvecs, r_tvecs, \ l2r_r, l2r_t, \ essential, fundamental
def stereo_handeye_calibration( l2r_rmat: np.ndarray, l2r_tvec: np.ndarray, left_ids: List, left_object_points: List, left_image_points: List, right_ids: List, right_image_points: List, left_camera_matrix: np.ndarray, left_camera_distortion: np.ndarray, right_camera_matrix: np.ndarray, right_camera_distortion: np.ndarray, device_tracking_array: List, calibration_tracking_array: List, left_rvecs: List[np.ndarray], left_tvecs: List[np.ndarray], right_rvecs: List[np.ndarray], right_tvecs: List[np.ndarray], quat_model2hand_array: List, trans_model2hand_array: List): """Wrapper around handeye calibration functions and reprojection / reconstruction error metrics. :param l2r_rmat: [3x3] ndarray, rotation for l2r transform :type l2r_rmat: np.ndarray :param l2r_tvec: [3x1] ndarray, translation for l2r transform :type l2r_tvec: np.ndarray :param left_ids: Vector of ndarrays containing integer point ids. :type left_ids: List :param left_object_points: Vector of Vector of 1x3 of type float32 :type left_object_points: List :param left_image_points: Vector of Vector of 1x2 of type float32 :type left_image_points: List :param right_ids: Vector of ndarrays containing integer point ids. :type right_ids: List :param right_image_points: Vector of Vector of 1x3 of type float32 :type right_image_points: List :param left_camera_matrix: Camera intrinsic matrix :type left_camera_matrix: np.ndarray :param left_camera_distortion: Camera distortion coefficients :type left_camera_distortion: np.ndarray :param right_camera_matrix: Camera intrinsic matrix :type right_camera_matrix: np.ndarray :param right_camera_distortion: Camera distortion coefficients :type right_camera_distortion: np.ndarray :param device_tracking_array: Tracking data for camera (hand) :type device_tracking_array: List :param calibration_tracking_array: Tracking data for calibration target :type calibration_tracking_array: List :param left_rvecs: Vector of 3x1 ndarray, Rodrigues rotations for each camera :type left_rvecs: List[np.ndarray] :param left_tvecs: Vector of [3x1] ndarray, translations for each camera :type left_tvecs: List[np.ndarray] :param right_rvecs: Vector of 3x1 ndarray, Rodrigues rotations for each camera :type right_rvecs: List[np.ndarray] :param right_tvecs: Vector of [3x1] ndarray, translations for each camera :type right_tvecs: List[np.ndarray] :param quat_model2hand_array: Array of model to hand quaternions :type quat_model2hand_array: List :param trans_model2hand_array: Array of model to hand translaions :type trans_model2hand_array: List :return: Reprojection error, reconstruction error, left handeye matrix, left pattern to marker matrix, right handeye, right pattern to marker :rtype: float, float, np.ndarray, np.ndarray, np.ndarray, np.ndarray """ # Do calibration, using Guofang's method on each left/right channel. left_handeye_matrix, left_pattern2marker_matrix = \ he.handeye_calibration(left_rvecs, left_tvecs, quat_model2hand_array, trans_model2hand_array) right_handeye_matrix, right_pattern2marker_matrix = \ he.handeye_calibration(right_rvecs, right_tvecs, quat_model2hand_array, trans_model2hand_array) # Filter common image points minimum_points = 10 _, common_object_pts, common_l_image_pts, common_r_image_pts = \ vu.filter_common_points_all_images( left_ids, left_object_points, left_image_points, right_ids, right_image_points, minimum_points) # Now try another optimisation. # Have one version of the pattern2marker matrix and handeye matrix, # and also optimise the tracking information to match. number_of_frames = len(common_object_pts) number_of_parameters = (number_of_frames * 6) + 12 x_0 = np.zeros(number_of_parameters) for i in range(0, number_of_frames): m2h = np.linalg.inv(device_tracking_array[i]) \ @ calibration_tracking_array[i] m2h_rvec, m2h_tvec = vu.extrinsic_matrix_to_vecs(m2h) x_0[i * 6 + 0] = m2h_rvec[0][0] x_0[i * 6 + 1] = m2h_rvec[1][0] x_0[i * 6 + 2] = m2h_rvec[2][0] x_0[i * 6 + 3] = m2h_tvec[0][0] x_0[i * 6 + 4] = m2h_tvec[1][0] x_0[i * 6 + 5] = m2h_tvec[2][0] p2m_rvec, p2m_tvec = vu.extrinsic_matrix_to_vecs( left_pattern2marker_matrix) x_0[number_of_frames * 6 + 0] = p2m_rvec[0][0] x_0[number_of_frames * 6 + 1] = p2m_rvec[1][0] x_0[number_of_frames * 6 + 2] = p2m_rvec[2][0] x_0[number_of_frames * 6 + 3] = p2m_tvec[0][0] x_0[number_of_frames * 6 + 4] = p2m_tvec[1][0] x_0[number_of_frames * 6 + 5] = p2m_tvec[2][0] h2e_rvec, h2e_tvec = vu.extrinsic_matrix_to_vecs(left_handeye_matrix) x_0[(number_of_frames + 1) * 6 + 0] = h2e_rvec[0][0] x_0[(number_of_frames + 1) * 6 + 1] = h2e_rvec[1][0] x_0[(number_of_frames + 1) * 6 + 2] = h2e_rvec[2][0] x_0[(number_of_frames + 1) * 6 + 3] = h2e_tvec[0][0] x_0[(number_of_frames + 1) * 6 + 4] = h2e_tvec[1][0] x_0[(number_of_frames + 1) * 6 + 5] = h2e_tvec[2][0] res = least_squares(vcf.stereo_handeye_error, x_0, args=(common_object_pts, common_l_image_pts, common_r_image_pts, left_camera_matrix, left_camera_distortion, right_camera_matrix, right_camera_distortion, l2r_rmat, l2r_tvec), method='lm', x_scale='jac', verbose=0) LOGGER.info("Stereo Handeye Re-Calibration: status=%s", str(res.status)) LOGGER.info("Stereo Handeye Re-Calibration: success=%s", str(res.success)) LOGGER.info("Stereo Handeye Re-Calibration: msg=%s", str(res.message)) # Extract data from result object. x_1 = res.x tmp_rvec = np.zeros((3, 1)) tmp_rvec[0][0] = x_1[number_of_frames * 6 + 0] tmp_rvec[1][0] = x_1[number_of_frames * 6 + 1] tmp_rvec[2][0] = x_1[number_of_frames * 6 + 2] tmp_tvec = np.zeros((3, 1)) tmp_tvec[0][0] = x_1[number_of_frames * 6 + 3] tmp_tvec[1][0] = x_1[number_of_frames * 6 + 4] tmp_tvec[2][0] = x_1[number_of_frames * 6 + 5] left_pattern2marker_matrix = vu.extrinsic_vecs_to_matrix( tmp_rvec, tmp_tvec) tmp_rvec[0][0] = x_1[(number_of_frames + 1) * 6 + 0] tmp_rvec[1][0] = x_1[(number_of_frames + 1) * 6 + 1] tmp_rvec[2][0] = x_1[(number_of_frames + 1) * 6 + 2] tmp_tvec[0][0] = x_1[(number_of_frames + 1) * 6 + 3] tmp_tvec[1][0] = x_1[(number_of_frames + 1) * 6 + 4] tmp_tvec[2][0] = x_1[(number_of_frames + 1) * 6 + 5] left_handeye_matrix = vu.extrinsic_vecs_to_matrix(tmp_rvec, tmp_tvec) l2r_matrix = skcm.construct_rigid_transformation(l2r_rmat, l2r_tvec) right_handeye_matrix = l2r_matrix @ left_handeye_matrix right_pattern2marker_matrix = copy.deepcopy(left_pattern2marker_matrix) # Remember that we have optimised tracking matrices. # So computation of error statistics should include these new positions. dummy_dt_array = [] dummy_ct_array = [] for i in range(0, number_of_frames): tmp_rvec[0][0] = x_1[i * 6 + 0] tmp_rvec[1][0] = x_1[i * 6 + 1] tmp_rvec[2][0] = x_1[i * 6 + 2] tmp_tvec[0][0] = x_1[i * 6 + 3] tmp_tvec[1][0] = x_1[i * 6 + 4] tmp_tvec[2][0] = x_1[i * 6 + 5] calib_track_mat = vu.extrinsic_vecs_to_matrix(tmp_rvec, tmp_tvec) device_track_mat = np.eye(4) dummy_dt_array.append(device_track_mat) dummy_ct_array.append(calib_track_mat) # Now compute some output statistics. sse, num_samples = vm.compute_stereo_2d_err_handeye( common_object_pts, common_l_image_pts, left_camera_matrix, left_camera_distortion, common_r_image_pts, right_camera_matrix, right_camera_distortion, dummy_dt_array, dummy_ct_array, left_handeye_matrix, left_pattern2marker_matrix, right_handeye_matrix, right_pattern2marker_matrix) mse = sse / num_samples reproj_err = np.sqrt(mse) sse, num_samples = vm.compute_stereo_3d_err_handeye( l2r_rmat, l2r_tvec, common_object_pts, common_l_image_pts, left_camera_matrix, left_camera_distortion, common_r_image_pts, right_camera_matrix, right_camera_distortion, dummy_dt_array, dummy_ct_array, left_handeye_matrix, left_pattern2marker_matrix, ) mse = sse / num_samples recon_err = np.sqrt(mse) return reproj_err, recon_err, \ left_handeye_matrix, left_pattern2marker_matrix, \ right_handeye_matrix, right_pattern2marker_matrix
def stereo_2d_and_3d_error(x_0, left_object_points, left_image_points, left_distortion, right_object_points, right_image_points, right_distortion): """ Private method to compute RMSE cost function, where x_0 contains the l2r rvec, l2r tvec, left intrinsics, right intrinsics, and left camera extrinsics. """ l2r_rvec = np.zeros((3, 1)) l2r_rvec[0][0] = x_0[0] l2r_rvec[1][0] = x_0[1] l2r_rvec[2][0] = x_0[2] l2r_rmat = (cv2.Rodrigues(l2r_rvec))[0] l2r_tvec = np.zeros((3, 1)) l2r_tvec[0][0] = x_0[3] l2r_tvec[1][0] = x_0[4] l2r_tvec[2][0] = x_0[5] left_camera = np.eye(3) left_camera[0][0] = x_0[6] left_camera[1][1] = x_0[7] left_camera[0][2] = x_0[8] left_camera[1][2] = x_0[9] right_camera = np.eye(3) right_camera[0][0] = x_0[10] right_camera[1][1] = x_0[11] right_camera[0][2] = x_0[12] right_camera[1][2] = x_0[13] sse = 0 number_of_samples = 0 number_of_frames = len(left_object_points) rvecs = [] tvecs = [] for i in range(0, number_of_frames): rvec = np.zeros((3, 1)) rvec[0][0] = x_0[14 + 6 * i + 0] rvec[1][0] = x_0[14 + 6 * i + 1] rvec[2][0] = x_0[14 + 6 * i + 2] tvec = np.zeros((3, 1)) tvec[0][0] = x_0[14 + 6 * i + 3] tvec[1][0] = x_0[14 + 6 * i + 4] tvec[2][0] = x_0[14 + 6 * i + 5] rvecs.append(rvec) tvecs.append(tvec) for i in range(0, number_of_frames): world_to_left_camera = vu.extrinsic_vecs_to_matrix(rvecs[0], tvecs[0]) if i > 0: offset = vu.extrinsic_vecs_to_matrix(rvecs[i], tvecs[i]) world_to_left_camera = np.matmul(offset, world_to_left_camera) rvec, tvec = vu.extrinsic_matrix_to_vecs(world_to_left_camera) rvecs[i] = rvec tvecs[i] = tvec tmp_sse, tmp_num = vm.compute_stereo_2d_err( l2r_rmat, l2r_tvec, left_object_points, left_image_points, left_camera, left_distortion, right_object_points, right_image_points, right_camera, right_distortion, rvecs, tvecs) sse = sse + tmp_sse number_of_samples = number_of_samples + tmp_num tmp_sse, tmp_num = \ vm.compute_stereo_3d_error(l2r_rmat, l2r_tvec, left_object_points, left_image_points, left_camera, left_distortion, right_image_points, right_camera, right_distortion, rvecs, tvecs ) sse = sse + tmp_sse number_of_samples = number_of_samples + tmp_num mse = sse / number_of_samples rmse = np.sqrt(mse) return rmse
def stereo_handeye_error(x_0, common_object_points, common_left_image_points, common_right_image_points, left_intrinsics, left_distortion, right_intrinsics, right_distortion, l2r_rmat, l2r_tvec): """ Cost function to return residual error. x_0 should contain an array of combined chessboard-marker-to-device-marker tracking 6DOF (rvec, tvec), chessboard-pattern-to-marker 6DOF and the device-hand-to-eye matrix 6DOF. :param x_0: :param common_object_points: :param common_left_image_points: :param common_right_image_points: :param left_intrinsics: :param left_distortion: :param right_intrinsics: :param right_distortion: :param l2r_rmat: :param l2r_tvec: :return: matrix of residuals for Levenberg-Marquardt optimisation. """ rvecs = [] tvecs = [] number_of_frames = len(common_object_points) p2m_rvec = np.zeros((3, 1)) p2m_rvec[0][0] = x_0[6 * number_of_frames + 0] p2m_rvec[1][0] = x_0[6 * number_of_frames + 1] p2m_rvec[2][0] = x_0[6 * number_of_frames + 2] p2m_tvec = np.zeros((3, 1)) p2m_tvec[0][0] = x_0[6 * number_of_frames + 3] p2m_tvec[1][0] = x_0[6 * number_of_frames + 4] p2m_tvec[2][0] = x_0[6 * number_of_frames + 5] h2e_rvec = np.zeros((3, 1)) h2e_rvec[0][0] = x_0[6 * (number_of_frames + 1) + 0] h2e_rvec[1][0] = x_0[6 * (number_of_frames + 1) + 1] h2e_rvec[2][0] = x_0[6 * (number_of_frames + 1) + 2] h2e_tvec = np.zeros((3, 1)) h2e_tvec[0][0] = x_0[6 * (number_of_frames + 1) + 3] h2e_tvec[1][0] = x_0[6 * (number_of_frames + 1) + 4] h2e_tvec[2][0] = x_0[6 * (number_of_frames + 1) + 5] p2m = vu.extrinsic_vecs_to_matrix(p2m_rvec, p2m_tvec) h2e = vu.extrinsic_vecs_to_matrix(h2e_rvec, h2e_tvec) for i in range(0, number_of_frames): m2h_rvec = np.zeros((3, 1)) m2h_rvec[0][0] = x_0[6 * i + 0] m2h_rvec[1][0] = x_0[6 * i + 1] m2h_rvec[2][0] = x_0[6 * i + 2] m2h_tvec = np.zeros((3, 1)) m2h_tvec[0][0] = x_0[6 * i + 3] m2h_tvec[1][0] = x_0[6 * i + 4] m2h_tvec[2][0] = x_0[6 * i + 5] m2h = vu.extrinsic_vecs_to_matrix(m2h_rvec, m2h_tvec) extrinsics = h2e @ m2h @ p2m rvec, tvec = vu.extrinsic_matrix_to_vecs(extrinsics) rvecs.append(rvec) tvecs.append(tvec) residual = vm.compute_stereo_2d_err(l2r_rmat, l2r_tvec, common_object_points, common_left_image_points, left_intrinsics, left_distortion, common_object_points, common_right_image_points, right_intrinsics, right_distortion, rvecs, tvecs, return_residuals=True) return residual