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.txtindex 4edbf837..c489e10d 100644--- a/requirements.txt+++ b/requirements.txt@@ -28,3 +28,6 @@ aiomultiprocess==0.9.0#viztracer==0.14.5pre-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 filediff --git a/src/core_processor/board_detection/board_detection.py b/src/core_processor/board_detection/board_detection.pyindex 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 rundiff --git a/src/core_processor/board_detection/detect_charuco_board.py b/src/core_processor/board_detection/detect_charuco_board.pyindex 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.pyindex 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_onumber_of_distortion_coefficients = 5 # may be 4, 5, 8, or 12, not sure of the trade-off there+@dataclassclass 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#ga69f2545a8b62a6b0fc2ee060dc30559dundistorted_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 errorhelpful 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__":