Free Motion Capture for Everyone πβ¨
TypeScript
janky per-frame lens distortion correction
Jon46c146cb39df
diff --git a/src/cameras/capture/opencv_camera/opencv_camera.py b/src/cameras/capture/opencv_camera/opencv_camera.pyindex b77abac2..51c4cf8f 100644--- a/src/cameras/capture/opencv_camera/opencv_camera.py+++ b/src/cameras/capture/opencv_camera/opencv_camera.py@@ -23,8 +23,8 @@ class WebcamConfig(BaseModel):resolution_width: int = 800resolution_height: int = 600save_video: bool = True- fourcc: str = "MP4V"- # fourcc: str = "MJPG"+ # fourcc: str = "MP4V"+ fourcc: str = "MJPG"base_save_video_dir = _get_home_dir()@@ -76,7 +76,7 @@ class OpenCVCamera:self._opencv_video_capture_object = cv2.VideoCapture(self._config.webcam_id, cap_backend)- # self._apply_configuration()+ self._apply_configuration()success, image = self._opencv_video_capture_object.read()if not success:@@ -129,6 +129,7 @@ class OpenCVCamera:self._opencv_video_capture_object.set(cv2.CAP_PROP_FRAME_HEIGHT, self._config.resolution_height)+self._opencv_video_capture_object.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*self._config.fourcc))diff --git a/src/core_processor/board_detection/base_pose_estimation.py b/src/core_processor/board_detection/base_pose_estimation.pydeleted file mode 100644index d0a43174..00000000--- a/src/core_processor/board_detection/base_pose_estimation.py+++ /dev/null@@ -1,51 +0,0 @@-from dataclasses import dataclass-from typing import List, Optional--import cv2-import numpy as np--from src.core_processor.board_detection.charuco_constants import aruco_dict, board--@dataclass-class CharucoData:- 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:- """- Charuco base pose estimation.- 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- criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)-- gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)- aruco_square_corners, aruco_square_ids, rejectedImgPoints = cv2.aruco.detectMarkers(- gray, aruco_dict- )-- if len(aruco_square_corners) > 0:- # SUB PIXEL DETECTION- for this_corner in aruco_square_corners:- cv2.cornerSubPix(- gray, this_corner, winSize=(3, 3), zeroZone=(-1, -1), criteria=criteria- )- res2 = cv2.aruco.interpolateCornersCharuco(- aruco_square_corners, aruco_square_ids, gray, board- )-- if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3:- charuco_corners = res2[1]- charuco_ids = res2[2]-- return CharucoData(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/board_detection/board_detection.py b/src/core_processor/board_detection/board_detection.pyindex 67c20ffd..e3d98e3e 100644--- a/src/core_processor/board_detection/board_detection.py+++ b/src/core_processor/board_detection/board_detection.py@@ -12,7 +12,7 @@ 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.base_pose_estimation import detect_charuco_board, CharucoData+from src.core_processor.board_detection.detect_charuco_board import detect_charuco_board, CharucoDatafrom src.core_processor.board_detection.charuco_image_annotator import (annotate_image_with_charuco_data,)@@ -25,7 +25,7 @@ logger = logging.getLogger(__name__)@dataclassclass CharucoFramePayload:raw_frame_payload: FramePayload- annotated_frame_payload: FramePayload+ annotated_frame_image: np.ndarraycharuco_data: CharucoDataclass BoardDetection:@@ -46,9 +46,9 @@ class BoardDetection:if not cv_cam.is_capturing_frames:returncharuco_frame_payload = self._detect_charuco_board_in_image(cv_cam)- if cb and charuco_frame_payload.annotated_frame_payload.image is not None:- writer.write(charuco_frame_payload.annotated_frame_payload)- await cb(charuco_frame_payload.annotated_frame_payload.image)+ if cb and charuco_frame_payload.annotated_frame_image is not None:+ writer.write(charuco_frame_payload.raw_frame_payload)+ await cb(charuco_frame_payload.annotated_frame_image)except Exception as e:logger.error("Printing traceback")traceback.print_exc()@@ -92,15 +92,16 @@ class BoardDetection:continueif save_video:- writer.write(charuco_frame_payload.annotated_frame_payload)+ writer.write(charuco_frame_payload.raw_frame_payload)if post_processed_frame_cb:- post_processed_frame_cb(current_webcam_id, charuco_frame_payload)+ undistorted_annotated_image = post_processed_frame_cb(current_webcam_id, charuco_frame_payload)+ charuco_frame_payload.annotated_frame_image = undistorted_annotated_imagefps_manager.increment_frame_processed_for(current_webcam_id)if show_window:should_continue = show_cam_window(- current_webcam_id, charuco_frame_payload.annotated_frame_payload.image, fps_manager+ current_webcam_id, charuco_frame_payload.annotated_frame_image, fps_manager)except:logger.error("Printing traceback")@@ -136,14 +137,17 @@ class BoardDetection:charuco_data = detect_charuco_board(raw_frame_payload.image)annotated_image = raw_frame_payload.image.copy()+ if charuco_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+ )+ cv2.polylines(annotated_image, np.int32([charuco_data.charuco_corners]), True, (0, 100, 255), 2)- success_bool = annotate_image_with_charuco_data(- annotated_image, cv_cam.webcam_id_as_str, charuco_data- )- cv2.polylines(annotated_image, np.int32([charuco_data.charuco_corners]), True, (0, 100, 255), 2)return CharucoFramePayload(- annotated_frame_payload=FramePayload(image=annotated_image, timestamp=time.time_ns()),+ annotated_frame_image=annotated_image,raw_frame_payload=raw_frame_payload,charuco_data=charuco_data)diff --git a/src/core_processor/board_detection/charuco_constants.py b/src/core_processor/board_detection/charuco_constants.pyindex 3816642c..2eda9a8d 100644--- a/src/core_processor/board_detection/charuco_constants.py+++ b/src/core_processor/board_detection/charuco_constants.py@@ -1,8 +1,8 @@import cv2-aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)-charuco_length = 7-charuco_width = 5+aruco_marker_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)+number_of_squares_width = 7+number_of_squares_height = 5-board = cv2.aruco.CharucoBoard_create(charuco_length, charuco_width, 1, 0.8, aruco_dict)-num_charuco_corners = (charuco_length - 1) * (charuco_width - 1)+charuco_board_object = cv2.aruco.CharucoBoard_create(number_of_squares_width, number_of_squares_height, 1, 0.8, aruco_marker_dict)+number_of_charuco_corners = (number_of_squares_width - 1) * (number_of_squares_height - 1)diff --git a/src/core_processor/board_detection/charuco_image_annotator.py b/src/core_processor/board_detection/charuco_image_annotator.pyindex ed183c7b..62b2d703 100644--- a/src/core_processor/board_detection/charuco_image_annotator.py+++ b/src/core_processor/board_detection/charuco_image_annotator.py@@ -3,20 +3,20 @@ from typing import Listimport cv2import numpy as np-from src.core_processor.board_detection.charuco_constants import num_charuco_corners+from src.core_processor.board_detection.charuco_constants import number_of_charuco_cornersdef _board_text(cam_name: str,charuco_ids: List[int],- charuco_corners: int,- charuco_detect_bool: bool,+ num_charuco_corners: int,+ full_board_detected_bool: bool,):- ids_as_str = str(len(charuco_ids))- corners_as_str = str(charuco_corners)+ num_ids_as_str = str(len(charuco_ids))+ num_corners_as_str = str(num_charuco_corners)text = (- f"{cam_name}: {ids_as_str} of {corners_as_str} Charuco Corner Points Detected "- f"| Full Board Detected: {charuco_detect_bool}"+ f"{cam_name}: {num_ids_as_str} of {num_corners_as_str} Charuco Corner Points Detected "+ f"| Full Board Detected: {full_board_detected_bool}")return text@@ -24,10 +24,7 @@ def _board_text(def annotate_image_with_charuco_data(image, webcam_id: str, charuco_data) -> bool:- full_charuco_detected_on_this_frame = False- if len(charuco_data.charuco_ids) == num_charuco_corners:- full_charuco_detected_on_this_frame = Trueimage_w_markers = cv2.aruco.drawDetectedCornersCharuco(image,@@ -43,8 +40,8 @@ def annotate_image_with_charuco_data(current_cam_corner_count_str = _board_text(this_cam_name,charuco_data.charuco_ids,- num_charuco_corners,- full_charuco_detected_on_this_frame,+ number_of_charuco_corners,+ charuco_data.full_board_found,)# TODO - Determine 'shared views' (i.e. frames in which a full board is detected by 2# cameras)@@ -77,13 +74,7 @@ def annotate_image_with_charuco_data(1,) # font stroke- if full_charuco_detected_on_this_frame:+ if charuco_data.full_board_found:cv2.polylines(image_w_markers, np.int32([charuco_data.charuco_corners]), False, (0, 100, 255), 2)- # for these_corners in charuco_points_from_previous_frames:- # if len(these_corners)>0:- # cv2.polylines(image_w_markers, np.int32([these_corners]), True, (0,100,255,- # 255/2), 2)-- return Truediff --git a/src/core_processor/board_detection/detect_charuco_board.py b/src/core_processor/board_detection/detect_charuco_board.pynew file mode 100644index 00000000..502d0574--- /dev/null+++ b/src/core_processor/board_detection/detect_charuco_board.py@@ -0,0 +1,75 @@+from dataclasses import dataclass+from typing import List, Optional++import cv2+import numpy as np++from src.core_processor.board_detection.charuco_constants import aruco_marker_dict, charuco_board_object, \+ number_of_charuco_corners+++@dataclass+class CharucoData:+ full_board_found: bool+ some_charuco_corners_found: bool+ any_markers_found: bool+ 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:+ """+ 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)diff --git a/src/core_processor/camera_calibration/camera_calibration.py b/src/core_processor/camera_calibration/camera_calibration.pyindex 06952714..6a4b8c6c 100644--- a/src/core_processor/camera_calibration/camera_calibration.py+++ b/src/core_processor/camera_calibration/camera_calibration.py@@ -1,20 +1,137 @@-from src.cameras.capture.frame_payload import FramePayload+from dataclasses import dataclass+import time+import cv2+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++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+ camera_matrix: np.ndarray = None+ lens_distortion_coefficients: np.ndarray = np.zeros((number_of_distortion_coefficients, 1))+ 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 = Noneclass CameraCalibration:def __init__(self, board_detection: BoardDetection):self._bd = board_detection+ self._current_calibration = SingleCameraCalibrationData()+ self._start_time = time.time()+ self._all_charuco_corners_list = []+ self._all_charuco_ids_list = []++def perform_calibration(self):self._bd.process(show_window=True,- save_video=True,+ save_video=False,post_processed_frame_cb=self._calibration_work,)def _calibration_work(self, webcam_id: str, charuco_frame_payload: CharucoFramePayload):- print(f"found frame from camera {webcam_id} at timestamp {charuco_frame_payload.annotated_frame_payload.timestamp/1e9}")+ 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)}')+ 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++ 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)}')++ # 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+ )++ # https://docs.opencv.org/4.5.5/d9/d0c/group__calib3d.html#ga69f2545a8b62a6b0fc2ee060dc30559d+ undistorted_image = cv2.undistort(charuco_frame_payload.annotated_frame_image,+ self._current_calibration.camera_matrix,+ self._current_calibration.lens_distortion_coefficients,+ None,+ new_camera_matrix)++ return undistorted_image+++ def calibrate_camera(self, charuco_corners_list, charuco_ids_list, image_width, image_height):+ """+ adapted from - https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html++ Calibrates the camera using the charuco data++ helpful resources -+ https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d <-opencv's 3d camera calibration docs+ https://www.adamsmith.haus/python/docs/cv2.aruco.calibrateCameraCharucoExtended+ https://www.mathworks.com/help/vision/ug/camera-calibration.html+ https://docs.opencv.org/3.4/d9/db7/tutorial_py_table_of_contents_calib3d.html+ https://calib.io/blogs/knowledge-base/camera-models <-a good one+ """+ 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.],+ [0., float(image_width), image_height / 2.],+ [0., 0., 1.]])++++ # 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_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)+if __name__ == "__main__":