Free Motion Capture for Everyone πβ¨
TypeScript
Pretty functional lens distortion calibrator, i think?
Jon5569c731a476
diff --git a/src/core_processor/board_detection/board_detection.py b/src/core_processor/board_detection/board_detection.pyindex 5d0f17a4..b2c5430e 100644--- a/src/core_processor/board_detection/board_detection.py+++ b/src/core_processor/board_detection/board_detection.py@@ -12,7 +12,9 @@ import numpy as npfrom src.cameras.capture.frame_payload import FramePayloadfrom src.cameras.multicam_manager.cv_camera_manager import CVCameraManagerfrom src.cameras.persistence.video_writer.video_writer import SaveOptions-from src.core_processor.board_detection.detect_charuco_board import detect_charuco_board, CharucoData+from src.core_processor.board_detection.charuco_constants import aruco_marker_dict, charuco_board_object, \+ number_of_charuco_corners+from src.core_processor.board_detection.detect_charuco_board import detect_charuco_board, CharucoViewDatafrom src.core_processor.board_detection.charuco_image_annotator import (annotate_image_with_charuco_data,)@@ -26,7 +28,7 @@ logger = logging.getLogger(__name__)class CharucoFramePayload:raw_frame_payload: FramePayloadannotated_frame_image: np.ndarray- charuco_data: CharucoData+ charuco_view_data: CharucoViewDataclass BoardDetection:def __init__(self, cam_manager: CVCameraManager = CVCameraManager()):@@ -134,25 +136,25 @@ class BoardDetection:if raw_frame_payload.image is None:logger.error("Frame is empty")return- charuco_data = detect_charuco_board(raw_frame_payload.image)+ charuco_view_data = detect_charuco_board(raw_frame_payload.image)annotated_image = raw_frame_payload.image.copy()- if charuco_data.some_charuco_corners_found:+ if charuco_view_data.some_charuco_corners_found:annotate_image_with_charuco_data(annotated_image, #this image will have stuff drawn on top of it inside this functioncv_cam.webcam_id_as_str,- charuco_data+ charuco_view_data)- cv2.polylines(annotated_image, np.int32([charuco_data.charuco_corners]), True, (0, 100, 255), 2)+ cv2.polylines(annotated_image, np.int32([charuco_view_data.charuco_corners]), True, (0, 100, 255), 2)return CharucoFramePayload(annotated_frame_image=annotated_image,raw_frame_payload=raw_frame_payload,- charuco_data=charuco_data+ charuco_view_data=charuco_view_data)- def detect_charuco_board(image) -> CharucoData:+ def detect_charuco_board(image) -> CharucoViewData:"""detect charuco board in an image.more-or-less copied from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html@@ -198,13 +200,13 @@ class BoardDetection:charuco_corners = res2[1]charuco_ids = res2[2]- return CharucoData(full_board_found=full_board_found,- some_charuco_corners_found=some_charuco_corners_found,- any_markers_found=any_markers_found,- charuco_corners=charuco_corners,- charuco_ids=charuco_ids,- aruco_square_corners=aruco_square_corners,- aruco_square_ids=aruco_square_ids)+ return CharucoViewData(full_board_found=full_board_found,+ some_charuco_corners_found=some_charuco_corners_found,+ any_markers_found=any_markers_found,+ charuco_corners=charuco_corners,+ charuco_ids=charuco_ids,+ aruco_square_corners=aruco_square_corners,+ aruco_square_ids=aruco_square_ids)if __name__ == "__main__":diff --git a/src/core_processor/board_detection/detect_charuco_board.py b/src/core_processor/board_detection/detect_charuco_board.pyindex 8d1a6c64..acab74fe 100644--- a/src/core_processor/board_detection/detect_charuco_board.py+++ b/src/core_processor/board_detection/detect_charuco_board.py@@ -9,17 +9,19 @@ from src.core_processor.board_detection.charuco_constants import aruco_marker_di@dataclass-class CharucoData:+class CharucoViewData:full_board_found: boolsome_charuco_corners_found: boolany_markers_found: bool+ image_width: int+ image_height: intcharuco_corners: Optional[np.ndarray]charuco_ids: Optional[np.ndarray]aruco_square_corners: Optional[np.ndarray]aruco_square_ids: Optional[np.ndarray]-def detect_charuco_board(image) -> CharucoData:+def detect_charuco_board(image) -> CharucoViewData:"""detect charuco board in an image.more-or-less copied from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html@@ -27,6 +29,7 @@ def detect_charuco_board(image) -> CharucoData:"""charuco_corners = []charuco_ids = []+ image_height, image_width, color_channels = image.shape# SUB PIXEL CORNER DETECTION CRITERIONtermination_criteria_threshold = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)@@ -64,11 +67,12 @@ def detect_charuco_board(image) -> CharucoData:charuco_corners = res2[1]charuco_ids = res2[2]-- return CharucoData(full_board_found=full_board_found,- some_charuco_corners_found=some_charuco_corners_found,- any_markers_found=any_markers_found,- charuco_corners=charuco_corners,- charuco_ids=charuco_ids,- aruco_square_corners=aruco_square_corners,- aruco_square_ids=aruco_square_ids)+ return CharucoViewData(full_board_found=full_board_found,+ some_charuco_corners_found=some_charuco_corners_found,+ any_markers_found=any_markers_found,+ image_width=image_width,+ image_height=image_height,+ charuco_corners=charuco_corners,+ charuco_ids=charuco_ids,+ aruco_square_corners=aruco_square_corners,+ aruco_square_ids=aruco_square_ids)diff --git a/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py b/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.pyindex 022d3be7..04eefd1b 100644--- a/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py+++ b/src/core_processor/camera_calibration/calibration_diagnostics_visualizer.py@@ -1,30 +1,43 @@import numpy as npimport pyqtgraph as pg-+import cv2class CalibrationDiagnosticsVisualizer:def __init__(self):self.graphics_layout_window = pg.GraphicsLayoutWidget(show=True)- self.graphics_layout_window.setWindowTitle('pyqtgraph example: Scrolling Plots')+ self.graphics_layout_window.setWindowTitle('Reprojection Error')self.reprojection_error_all = np.empty(0)+ self.reprojection_error_current_best = np.empty(0)++ # create image subplot (based on `GraphicsItems - ImageItem - video` example `python -m pyqtgraph.examples`)+ self.undistorted_image_subplot = self.graphics_layout_window.addViewBox()+ self.undistorted_image_subplot.setAspectLocked(True)+ self.image_item_holder = pg.ImageItem(border='w')+ self.undistorted_image_subplot.addItem(self.image_item_holder)+ # create reprojection error sublotself.reprojection_error_subplot = self.graphics_layout_window.addPlot()- self.reprojection_error_line = self.reprojection_error_subplot.plot(self.reprojection_error_all)+ self.reprojection_error_subplot.setLabel('left', "Reprojection Error", units='pixels')+ self.reprojection_error_subplot.setLabel('bottom', "Time", units='Frame#')+ self.reprojection_error_subplot.setYRange(0, 2)- self.subplot_1 = self.graphics_layout_window.addPlot()- self.data1 = np.random.normal(size=300)- self.curve2 = self.subplot_1.plot(self.data1)- self.ptr1 = 0+ self.reprojection_error_all_line = self.reprojection_error_subplot.plot(self.reprojection_error_all,+ pen=(255, 0, 128),+ name="reprojection error- all")- def update(self, reprojection_error_current):+ self.reprojection_error_current_best_line = self.reprojection_error_subplot.plot(self.reprojection_error_current_best)++ def update_reprojection_error_subplot(self, reprojection_error_new, reprojection_error_current_best):# update reprojection error subplot- self.reprojection_error_all = np.append(self.reprojection_error_all, reprojection_error_current)- self.reprojection_error_line.setData(self.reprojection_error_all)+ self.reprojection_error_all = np.append(self.reprojection_error_all, reprojection_error_new)+ self.reprojection_error_current_best = np.append(self.reprojection_error_current_best, reprojection_error_current_best)++ self.reprojection_error_all_line.setData(self.reprojection_error_all)+ self.reprojection_error_current_best_line.setData(self.reprojection_error_current_best)- # update other subplot- self.curve2.setData(self.data1)- self.curve2.setPos(self.ptr1, 0)+ def update_image_subplot(self, new_image):+ self.image_item_holder.setImage(cv2.cvtColor(cv2.rotate(new_image, cv2.ROTATE_90_CLOCKWISE), cv2.COLOR_BGR2RGB))def close(self):self.graphics_layout_window.close()diff --git a/src/core_processor/camera_calibration/camera_calibration.py b/src/core_processor/camera_calibration/camera_calibration.pyindex 378b1601..fc2866fd 100644--- a/src/core_processor/camera_calibration/camera_calibration.py+++ b/src/core_processor/camera_calibration/camera_calibration.py@@ -1,34 +1,38 @@from dataclasses import dataclassimport timeimport cv2+import matplotlib.pyplot as pltimport pyqtgraph as pgimport numpy as npfrom rich import printfrom src.core_processor.board_detection.board_detection import BoardDetection, CharucoFramePayloadfrom src.core_processor.board_detection.charuco_constants import charuco_board_object+from src.core_processor.board_detection.detect_charuco_board import CharucoViewDatafrom src.core_processor.camera_calibration.calibration_diagnostics_visualizer import CalibrationDiagnosticsVisualizer@dataclassclass SingleCameraCalibrationData:- reprojection_error: float = 1e9+ reprojection_error: float = Nonecamera_matrix: np.ndarray = Nonelens_distortion_coefficients: np.ndarray = None+ image_width: int = None+ image_height: int = Nonerotation_vectors_of_the_board: np.ndarray = Nonetranslation_vectors_of_the_board: np.ndarray = Nonelens_distortion_std_dev: float = Nonecamera_location_std_dev: float = None-class CameraCalibration:def __init__(self, board_detection: BoardDetection, visualize_calibration_disagnostics=True, ):self._bd = board_detectionself._current_calibration = SingleCameraCalibrationData()self._start_time = time.time()- self._all_charuco_corners_list = []- self._all_charuco_ids_list = []+ self._all_charuco_views = []+ self._num_charuco_views = 0+ self._best_combo_of_charuco_views = []if visualize_calibration_disagnostics:self.calibration_diagnostics_visualizer = CalibrationDiagnosticsVisualizer()@@ -43,43 +47,44 @@ class CameraCalibration:def _calibration_work(self, webcam_id: str, charuco_frame_payload: CharucoFramePayload):elapsed_time = (charuco_frame_payload.raw_frame_payload.timestamp / 1e9) - self._start_time- image_width, image_height, color_channels = charuco_frame_payload.raw_frame_payload.image.shape-- if not charuco_frame_payload.charuco_data.full_board_found and self._current_calibration.camera_matrix is None:+ if not charuco_frame_payload.charuco_view_data.full_board_found and self._current_calibration.camera_matrix is None:return charuco_frame_payload.annotated_frame_image- if charuco_frame_payload.charuco_data.full_board_found:- self._all_charuco_corners_list.append(charuco_frame_payload.charuco_data.charuco_corners)- self._all_charuco_ids_list.append(charuco_frame_payload.charuco_data.charuco_ids)+ if charuco_frame_payload.charuco_view_data.full_board_found:+ new_charuco_view = charuco_frame_payload.charuco_view_data+ self._all_charuco_views.append(new_charuco_view)+ self._num_charuco_views = len(self._all_charuco_views)- self.estimate_lens_distortion(image_width, image_height, )+ if self._num_charuco_views < 2:+ return charuco_frame_payload.annotated_frame_image- # create new camera matrix that will show full undistored image (with black pixels in spots with no data (only for debugging)- new_camera_matrix, valid_ROI_lbwh = cv2.getOptimalNewCameraMatrix(self._current_calibration.camera_matrix,- self._current_calibration.lens_distortion_coefficients,- (image_width, image_height),- 1,- centerPrincipalPoint=True- )+ self.estimate_lens_distortion(new_charuco_view)- # https://docs.opencv.org/4.5.5/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d- undistorted_image = cv2.undistort(charuco_frame_payload.annotated_frame_image,++ # raw_image = charuco_frame_payload.raw_frame_payload.image+ raw_image = charuco_frame_payload.annotated_frame_image++ if self._current_calibration.camera_matrix is None:+ return raw_image++ undistorted_image_for_debug = self.undistort_image_with_invalid_pixels_as_black(raw_image,+ self._current_calibration)++ self.calibration_diagnostics_visualizer.update_image_subplot(undistorted_image_for_debug)++ undistorted_image = cv2.undistort(raw_image,self._current_calibration.camera_matrix,self._current_calibration.lens_distortion_coefficients,- None,- new_camera_matrix)-- self.calibration_diagnostics_visualizer.update(self._current_calibration.reprojection_error)+ )print(- f"found frame from camera {webcam_id} at timestamp {elapsed_time:.3f} - reprojection error: {self._current_calibration.reprojection_error:.3f} - num charuco views:{len(self._all_charuco_corners_list)}")- # print(f'lens distortion coefficients - {np.array_str(np.squeeze(self._current_calibration.lens_distortion_coefficients), precision=2)}')- # print(f'camera matrix - \n{np.array_str(self._current_calibration.camera_matrix, precision=2)}')-+ f"found frame from camera {webcam_id} at timestamp {elapsed_time:.3f} - reprojection error: {self._current_calibration.reprojection_error:.3f} - num charuco views:{len(self._all_charuco_views)}")return undistorted_image- def estimate_lens_distortion(self, image_width, image_height,- max_iterations=5, max_charuco_board_views=10):+ def estimate_lens_distortion(self,+ new_charuco_view: CharucoViewData,+ max_iterations=10,+ max_charuco_board_views=5) -> SingleCameraCalibrationData:"""adapted from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html@@ -94,66 +99,212 @@ class CameraCalibration:"""print("CAMERA CALIBRATION")+ if not self._current_calibration.reprojection_error:+ current_best_reprojection_error = 1e9 # put a big fake number on the first frame+ else:+ current_best_reprojection_error = self._current_calibration.reprojection_error++ image_width = new_charuco_view.image_width+ image_height = new_charuco_view.image_height+initial_camera_matrix = np.array([[float(image_width), 0., image_width / 2.],[0., float(image_width), image_height / 2.],[0., 0., 1.]])number_of_distortion_coefficients = 4 # may be 4, 5, 8, or 12, not sure of the trade-off there but low numbers seem more stableinitial_lens_distortion_coefficients = np.zeros((number_of_distortion_coefficients, 1))- num_charuco_views = len(self._all_charuco_ids_list)+ num_charuco_views_per_combo = 10+ if self._num_charuco_views < num_charuco_views_per_combo:+ list_of_combos_of_charuco_views = [self._all_charuco_views]+ else:+ list_of_combos_of_charuco_views = self.generate_combos_of_charuco_views(new_charuco_view)# flag definitions -> https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65dflags = (cv2.CALIB_USE_INTRINSIC_GUESS +- # cv2.CALIB_ZERO_TANGENT_DIST ++ cv2.CALIB_ZERO_TANGENT_DIST +cv2.cv2.CALIB_FIX_ASPECT_RATIO +cv2.CALIB_FIX_PRINCIPAL_POINT +cv2.CALIB_RATIONAL_MODEL# cv2.CALIB_THIN_PRISM_MODEL# cv2.CALIB_TILTED_MODEL)- # flags = (cv2.CALIB_RATIONAL_MODEL)-- for iter in range(max_iterations):-- if num_charuco_views < 2:- these_charuco_corners = self._all_charuco_corners_list- these_charuco_ids = self._all_charuco_ids_list- else:- this_random_sample_of_charuco_views = np.random.choice(num_charuco_views,- size=min(num_charuco_views,- max_charuco_board_views),- replace=False)- these_charuco_corners = [self._all_charuco_corners_list[cc] for cc in- this_random_sample_of_charuco_views]- these_charuco_ids = [self._all_charuco_ids_list[cc] for cc in this_random_sample_of_charuco_views]-- # https://docs.opencv.org/4.x/d9/d6a/group__aruco.html#gacf03e5afb0bc516b73028cf209984a06- (reprojection_error,- camera_matrix,- lens_distortion_coefficients,- rotation_vectors_of_the_board,- translation_vectors_of_the_board,- lens_distortion_std_dev,- camera_location_std_dev,- reprojection_error_per_view- ) = cv2.aruco.calibrateCameraCharucoExtended(- charucoCorners=these_charuco_corners,- charucoIds=these_charuco_ids,- board=charuco_board_object,- imageSize=(image_width, image_height),- cameraMatrix=initial_camera_matrix,- distCoeffs=initial_lens_distortion_coefficients,- flags=flags,- criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))-- if reprojection_error < self._current_calibration.reprojection_error:- self._current_calibration = SingleCameraCalibrationData(reprojection_error=reprojection_error,- camera_matrix=camera_matrix,- lens_distortion_coefficients=lens_distortion_coefficients,- rotation_vectors_of_the_board=rotation_vectors_of_the_board,- translation_vectors_of_the_board=translation_vectors_of_the_board,- lens_distortion_std_dev=lens_distortion_std_dev,- camera_location_std_dev=camera_location_std_dev)++ for this_combo_of_charuco_views in list_of_combos_of_charuco_views:+ for this_charuco_view in this_combo_of_charuco_views:+ # https://docs.opencv.org/4.x/d9/d6a/group__aruco.html#gacf03e5afb0bc516b73028cf209984a06+ (this_reprojection_error,+ this_camera_matrix,+ these_lens_distortion_coefficients,+ these_rotation_vectors_of_the_board,+ these_translation_vectors_of_the_board,+ this_lens_distortion_std_dev,+ this_camera_location_std_dev,+ these_reprojection_error_per_view+ ) = cv2.aruco.calibrateCameraCharucoExtended(+ charucoCorners=[this_charuco_view.charuco_corners],+ charucoIds=[this_charuco_view.charuco_ids],+ board=charuco_board_object,+ imageSize=(image_width, image_height),+ cameraMatrix=initial_camera_matrix,+ distCoeffs=initial_lens_distortion_coefficients,+ flags=flags,+ criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))++ self.calibration_diagnostics_visualizer.update_reprojection_error_subplot(this_reprojection_error,+ current_best_reprojection_error)++ if this_reprojection_error < current_best_reprojection_error:++ this_calibration = SingleCameraCalibrationData(+ reprojection_error=this_reprojection_error,+ camera_matrix=this_camera_matrix,+ lens_distortion_coefficients=these_lens_distortion_coefficients,+ image_width=image_width,+ image_height=image_height,+ rotation_vectors_of_the_board=these_rotation_vectors_of_the_board,+ translation_vectors_of_the_board=these_translation_vectors_of_the_board,+ lens_distortion_std_dev=this_lens_distortion_std_dev,+ camera_location_std_dev=this_camera_location_std_dev)++ if self.is_this_calibration_valid(this_calibration):+ self._current_calibration = this_calibration++ def generate_combos_of_charuco_views(self,+ new_charuco_view,+ num_combos_to_generate=10,+ num_views_per_combo=5):+ """+ return -+ 1x current best combo plus new view+ num_combos_to_generate/2x random combo WITH new view appended+ num_combos_to_generate/2x random combo WITHOUT new view appended+ """++ list_of_combos_of_charuco_views = []++ best_combo_plus_new = self._best_combo_of_charuco_views.copy()+ best_combo_plus_new.append(new_charuco_view)+ list_of_combos_of_charuco_views.append(best_combo_plus_new)++ for this_iter in range(num_combos_to_generate):+ this_random_sample_of_charuco_views = np.random.choice(num_views_per_combo,+ size=min(len(self._all_charuco_views),+ num_views_per_combo),+ replace=False)++ this_combo = [self._all_charuco_views[cc] for cc in this_random_sample_of_charuco_views]++ if this_iter < np.ceil(num_views_per_combo / 2):+ this_combo.append(new_charuco_view)++ list_of_combos_of_charuco_views.append(this_combo)++ return list_of_combos_of_charuco_views++ def is_this_calibration_valid(self, this_calibration):++ is_valid = False+ if not self.lens_distortion_is_monotonic(this_calibration):+ return False++ if self.too_many_invalid_pixels(this_calibration):+ return False++ return True++ def lens_distortion_is_monotonic(self, this_calibration) -> bool:+ """+ check if the lens distortion coefficients are valid by checking it the resulting undistorted image is monotonically increasing along each diagonal (i.e. the image doesn't wrap around itself)++ per opencv calib3d docs, calibrations are valid if -++ 1 + k1*r^2 + k2*r^r + k3*r6++ is monotonically increasing (pincushion distortion) or monotonically decreasing (barrel distortion).++ non-monotonic behavior means the calibration is invalid, so we check for that by examining the `np.diff` of a vector of points extending from image center to the top left coners to make sure it never goes negative(NOTE - check other directions?)++ since most cameras are wide-angle (i.e. they give a wider field of view than a pinhole camera), we may constrain things further be requiring things be monotonically increasing (i.e. only return pincushion distortions)+++ see -+ https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html <-mathematical defintion of lens distortion+ https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <- docs explaining lens_distortion_coefficients+ https://github.com/opencv/opencv/issues/15992 <-conversation about the fact that opencv doesn't check for this kinda thing++ """+ calibration_valid = False++ pixel_distance_to_image_corner = np.sqrt(np.power(this_calibration.image_width, 2)+ + np.power(this_calibration.image_height, 2))++ distances_from_image_center = np.linspace(-pixel_distance_to_image_corner,+ pixel_distance_to_image_corner) # aka, the `r` as defined in teh docs++ # following naming conventions+ k1 = this_calibration.lens_distortion_coefficients[0]+ k2 = this_calibration.lens_distortion_coefficients[1]+ p1 = this_calibration.lens_distortion_coefficients[2] # I think p1 and p2 are for tangential distortion...+ p2 = this_calibration.lens_distortion_coefficients[3] # ...which I might want to check on later?+ k3 = this_calibration.lens_distortion_coefficients[4]++ distorted_points = 1 + \+ k1 * np.power(distances_from_image_center, 2) + \+ k2 * np.power(distances_from_image_center, 4) + \+ k3 * np.power(distances_from_image_center, 6)++ diff_distorted_points = np.diff(distances_from_image_center)++ # if show_debug_plot:+ # fig = plt.Figure()+ # fig.suptitle('is_calibration_valid - debug plot')+ # plt.plot(distances_from_image_center, label='Distance from center of image')+ # plt.plot(distorted_points, label='Distorted points')+ # plt.legend(loc="upper left")+ # plt.show()++ if np.sum(diff_distorted_points < 0) == 0:+ print(f'Yay! Monotonic! k1={k1}, k2={k2}, k3={k3}')+ return True++ print(f'Boo! Non-monotonic! k1={k1}, k2={k2}, k3={k3}')+ return False++ def too_many_invalid_pixels(self, this_calibration, how_many_is_too_many=.75) -> bool:+ """+ create new camera matrix that will show full undistored image with black pixels in spots with no data.+ return FALSE if black pixels are greater than `how_many_is_too_many` proportion of the full image+ """++ dummy_image = np.ones((this_calibration.image_width, this_calibration.image_height), np.uint8) * 255++ undistorted_image = self.undistort_image_with_invalid_pixels_as_black(dummy_image, this_calibration)++ num_invalid_pixels = np.sum(undistorted_image == 0)+ proportion_invalid_pixels = num_invalid_pixels / dummy_image.size++ if proportion_invalid_pixels > how_many_is_too_many:+ return True+ return False++ @staticmethod+ def undistort_image_with_invalid_pixels_as_black(image, calibration):+ new_camera_matrix, valid_ROI_lbwh = cv2.getOptimalNewCameraMatrix(+ calibration.camera_matrix,+ calibration.lens_distortion_coefficients,+ (calibration.image_width, calibration.image_height),+ 1,+ centerPrincipalPoint=True+ )++ # https://docs.opencv.org/4.5.5/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d+ undistorted_image = cv2.undistort(image,+ calibration.camera_matrix,+ calibration.lens_distortion_coefficients,+ None,+ new_camera_matrix)++ return undistorted_imageif __name__ == "__main__":