Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

moving torwards a pretty slick lens distortion calibrator, I gotta say

Jonb24a74c7fdd0
diff --git a/requirements.txt b/requirements.txt
index 4edbf837..c489e10d 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -28,3 +28,6 @@ aiomultiprocess==0.9.0
#viztracer==0.14.5
pre-commit==2.17.0
# Requirements for core-processor
+#pyqtgraph==0.12.4
+#pyqt5==5.15.6
+#pyopengl==3.1.6
\ No newline at end of file
diff --git a/src/core_processor/board_detection/board_detection.py b/src/core_processor/board_detection/board_detection.py
index e3d98e3e..5d0f17a4 100644
--- a/src/core_processor/board_detection/board_detection.py
+++ b/src/core_processor/board_detection/board_detection.py
@@ -152,6 +152,60 @@ class BoardDetection:
charuco_data=charuco_data
)
+ def detect_charuco_board(image) -> CharucoData:
+ """
+ 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
+
+ """
+ charuco_corners = []
+ charuco_ids = []
+
+ # SUB PIXEL CORNER DETECTION CRITERION
+ termination_criteria_threshold = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
+
+ grayscale_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+ aruco_square_corners, aruco_square_ids, rejected_image_points = cv2.aruco.detectMarkers(
+ grayscale_image, aruco_marker_dict
+ )
+
+ full_board_found = False
+ any_markers_found = False
+ some_charuco_corners_found = False
+
+ if len(aruco_square_corners) > 0:
+ any_markers_found = True
+ # refine detected corner locations to provide sub-pixel precision
+ # https://docs.opencv.org/4.x/dd/d1a/group__imgproc__feature.html#ga354e0d7c86d0d9da75de9b9701a9a87e
+ for this_corner in aruco_square_corners:
+ cv2.cornerSubPix(
+ grayscale_image, this_corner, winSize=(3, 3), zeroZone=(-1, -1),
+ criteria=termination_criteria_threshold
+ )
+ res2 = cv2.aruco.interpolateCornersCharuco(aruco_square_corners,
+ aruco_square_ids,
+ grayscale_image,
+ charuco_board_object
+ )
+
+ if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:
+ some_charuco_corners_found = True
+
+ if len(res2[2]) == number_of_charuco_corners:
+ full_board_found = True
+
+ 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)
+
if __name__ == "__main__":
## Jon, Run me to easily start a charuco board detect run
diff --git a/src/core_processor/board_detection/detect_charuco_board.py b/src/core_processor/board_detection/detect_charuco_board.py
index 502d0574..8d1a6c64 100644
--- a/src/core_processor/board_detection/detect_charuco_board.py
+++ b/src/core_processor/board_detection/detect_charuco_board.py
@@ -65,7 +65,6 @@ def detect_charuco_board(image) -> CharucoData:
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,
diff --git a/src/core_processor/camera_calibration/camera_calibration.py b/src/core_processor/camera_calibration/camera_calibration.py
index 6a4b8c6c..bf1b2b88 100644
--- a/src/core_processor/camera_calibration/camera_calibration.py
+++ b/src/core_processor/camera_calibration/camera_calibration.py
@@ -9,6 +9,7 @@ from src.core_processor.board_detection.charuco_constants import charuco_board_o
number_of_distortion_coefficients = 5 # may be 4, 5, 8, or 12, not sure of the trade-off there
+
@dataclass
class SingleCameraCalibrationData:
reprojection_error: float = 1e9
@@ -29,7 +30,6 @@ class CameraCalibration:
self._all_charuco_corners_list = []
self._all_charuco_ids_list = []
-
def perform_calibration(self):
self._bd.process(
show_window=True,
@@ -39,31 +39,25 @@ 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
- print(
- f"found frame from camera {webcam_id} at timestamp {elapsed_time:.3f}")
- if not charuco_frame_payload.charuco_data.full_board_found:
- print(f'reprojection error - {self._current_calibration.reprojection_error:.3f}')
- print(f'lens distortion coefficients - {np.array_str(np.squeeze(self._current_calibration.lens_distortion_coefficients), precision=2)}')
+ 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:
return charuco_frame_payload.annotated_frame_image
- charuco_data = charuco_frame_payload.charuco_data
- image_width, image_height, color_channels = charuco_frame_payload.raw_frame_payload.image.shape
+ 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)
- self._current_calibration = self.calibrate_camera(charuco_data.charuco_corners,
- charuco_data.charuco_ids,
- image_width,
- image_height,
- )
- print(f'camera matrix - \n{np.array_str(self._current_calibration.camera_matrix, precision=2)}')
+ self.estimate_lens_distortion(image_width, image_height,)
# 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._current_calibration.lens_distortion_coefficients,
+ (image_width, image_height),
+ 1,
+ centerPrincipalPoint=True
+ )
# https://docs.opencv.org/4.5.5/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d
undistorted_image = cv2.undistort(charuco_frame_payload.annotated_frame_image,
@@ -72,14 +66,19 @@ class CameraCalibration:
None,
new_camera_matrix)
- return undistorted_image
+ 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)}')
+ return undistorted_image
- def calibrate_camera(self, charuco_corners_list, charuco_ids_list, image_width, image_height):
+ def estimate_lens_distortion(self, image_width, image_height,
+ max_iterations=10, max_charuco_board_views=5):
"""
adapted from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html
- Calibrates the camera using the charuco data
+ Calibrates the camera using the charuco data using a RANSAC-like method - randomly selects board views and saves the combo that minimized reprojection error
helpful resources -
https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <-opencv's 3d camera calibration docs
@@ -90,48 +89,56 @@ class CameraCalibration:
"""
print("CAMERA CALIBRATION")
- if self._current_calibration.camera_matrix is None:
- self._current_calibration.camera_matrix = np.array([[float(image_width), 0., image_width / 2.],
+ initial_camera_matrix = np.array([[float(image_width), 0., image_width / 2.],
[0., float(image_width), image_height / 2.],
[0., 0., 1.]])
-
+ num_charuco_views = len(self._all_charuco_ids_list)
# flag definitions -> https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d
- flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_FIX_PRINCIPAL_POINT + cv2.CALIB_TILTED_MODEL)
+ flags = (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)
- # 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=charuco_corners_list,
- charucoIds=charuco_ids_list,
- board=charuco_board_object,
- imageSize=(image_width, image_height),
- cameraMatrix=self._current_calibration.camera_matrix.copy(),
- distCoeffs=self._current_calibration.lens_distortion_coefficients.copy(),
- flags=flags,
- criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))
-
- if reprojection_error > self._current_calibration.reprojection_error:
- return self._current_calibration
-
- return 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 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=np.zeros((number_of_distortion_coefficients, 1)),
+ 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)
if __name__ == "__main__":