Skip to content

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.py
index 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 np
from src.cameras.capture.frame_payload import FramePayload
from src.cameras.multicam_manager.cv_camera_manager import CVCameraManager
from 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, CharucoViewData
from 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: FramePayload
annotated_frame_image: np.ndarray
- charuco_data: CharucoData
+ charuco_view_data: CharucoViewData
class 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 function
cv_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.py
index 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: bool
some_charuco_corners_found: bool
any_markers_found: bool
+ image_width: int
+ image_height: int
charuco_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 CRITERION
termination_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.py
index 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 np
import pyqtgraph as pg
-
+import cv2
class 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 sublot
self.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.py
index 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 dataclass
import time
import cv2
+import matplotlib.pyplot as plt
import pyqtgraph as pg
import numpy as np
from rich import print
from src.core_processor.board_detection.board_detection import BoardDetection, CharucoFramePayload
from src.core_processor.board_detection.charuco_constants import charuco_board_object
+from src.core_processor.board_detection.detect_charuco_board import CharucoViewData
from src.core_processor.camera_calibration.calibration_diagnostics_visualizer import CalibrationDiagnosticsVisualizer
@dataclass
class SingleCameraCalibrationData:
- reprojection_error: float = 1e9
+ reprojection_error: float = None
camera_matrix: np.ndarray = None
lens_distortion_coefficients: np.ndarray = None
+ image_width: int = None
+ image_height: int = None
rotation_vectors_of_the_board: np.ndarray = None
translation_vectors_of_the_board: np.ndarray = None
lens_distortion_std_dev: float = None
camera_location_std_dev: float = None
-
class CameraCalibration:
def __init__(self, board_detection: BoardDetection, visualize_calibration_disagnostics=True, ):
self._bd = board_detection
self._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 stable
initial_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#ga3207604e4b1a1758aa66acb6ed5aa65d
flags = (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_image
if __name__ == "__main__":