Free Motion Capture for Everyone πβ¨
TypeScript
stash before switch to Pupil_2
Jon45aac8ef79c6
diff --git a/src/core_processor/camera_calibration/camera_calibrator.py b/src/core_processor/camera_calibration/camera_calibrator.pyindex a1a7b207..d3776dfb 100644--- a/src/core_processor/camera_calibration/camera_calibrator.py+++ b/src/core_processor/camera_calibration/camera_calibrator.py@@ -100,7 +100,7 @@ class CameraCalibrator:) = cv2.aruco.calibrateCameraCharucoExtended(charucoCorners=[charuco_view.charuco_corners],charucoIds=[charuco_view.charuco_ids],- board=self._charuco_board_detector.charuco_board,+ board=self._charuco_board_detector.cv2_aruco_charuco_board,imageSize=(image_width, image_height),cameraMatrix=self._current_calibration.camera_matrix,distCoeffs=self._current_calibration.lens_distortion_coefficients,diff --git a/src/core_processor/camera_calibration/lens_distortion_calibrator.py b/src/core_processor/camera_calibration/lens_distortion_calibrator.pyindex 5e21df22..ae0a45b9 100644--- a/src/core_processor/camera_calibration/lens_distortion_calibrator.py+++ b/src/core_processor/camera_calibration/lens_distortion_calibrator.py@@ -127,7 +127,7 @@ class LensDistortionCalibrator:) = cv2.aruco.calibrateCameraCharucoExtended(charucoCorners=[this_charuco_view.charuco_corners],charucoIds=[this_charuco_view.charuco_ids],- board=self._board_detector.charuco_board,+ board=self._board_detector.cv2_aruco_charuco_board,imageSize=(image_width, image_height),cameraMatrix=default_calibration.camera_matrix,distCoeffs=default_calibration.lens_distortion_coefficients,diff --git a/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.pyindex 06deb1d8..6827838a 100644--- a/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py+++ b/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py@@ -9,7 +9,8 @@ from src.config.home_dir import get_session_path, get_freemocap_data_folder_pathfrom src.pipelines.calibration_pipeline.anipose_camera_calibration import freemocap_aniposefrom aniposelib.boards import CharucoBoard as AniposeCharucoBoard-from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoard+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import \+ CharucoBoardDataClasslogger = logging.getLogger(__name__)@@ -17,15 +18,16 @@ logger = logging.getLogger(__name__)class AniposeCameraCalibrator:def __init__(self,session_id: str,- charuco_board_object: CharucoBoard,- charuco_square_size:Union[int,float] = 1,+ charuco_board_object: CharucoBoardDataClass,+ charuco_square_size: Union[int, float] = 1,):self._session_id = session_idself._charuco_board_object = charuco_board_objectif charuco_square_size == 1:- logger.warning('Charuco square size is not set, so units of 3d reconstructed data will be in units of `however_long_the_black_edge_of_the_charuco_square_was`. Please input `charuco_square_size` in millimeters (or your preferred unity of length)')+ logger.warning(+ 'Charuco square size is not set, so units of 3d reconstructed data will be in units of `however_long_the_black_edge_of_the_charuco_square_was`. Please input `charuco_square_size` in millimeters (or your preferred unity of length)')self._charuco_square_size = charuco_square_sizeself.get_paths_and_whatnot()self.initialize_anipose_objects()@@ -46,7 +48,7 @@ class AniposeCameraCalibrator:marker_bits=4,dict_size=250)- def calibrate_camera_capture_volume(self):+ def calibrate_camera_capture_volume(self, pin_camera_0_to_origin:bool=False):# anipose needs this to be a list of lists (which is annoying but whatevs)video_paths_list_of_list_of_strings = [[str(this_path)] for this_path in self._list_of_video_paths]@@ -56,15 +58,14 @@ class AniposeCameraCalibrator:logger.info("Anipose Calibration Successful!")- # translate cameras so camera0 is on `0,0,0`- self._anipose_camera_group_object = self.pin_camera_zero_to_origin(self._anipose_camera_group_object)+ if pin_camera_0_to_origin:+ # translate cameras so camera0 is on `0,0,0`+ self._anipose_camera_group_object = self.pin_camera_zero_to_origin(self._anipose_camera_group_object)# save calibration info to filescalibration_toml_filename = f"{self._session_id}_camera_calibration.toml"camera_calibration_toml_path = self.session_folder_path / calibration_toml_filename--self._anipose_camera_group_object.dump(camera_calibration_toml_path)logger.info(f"anipose camera calibration data saved to {str(camera_calibration_toml_path)}")@@ -90,15 +91,17 @@ class AniposeCameraCalibrator:def pin_camera_zero_to_origin(self, _anipose_camera_group_object):original_translation_vectors = _anipose_camera_group_object.get_translations()- camera_0_translation = original_translation_vectors[0,:]+ camera_0_translation = original_translation_vectors[0, :]altered_translation_vectors = np.zeros(original_translation_vectors.shape)for this_camera_number in range(original_translation_vectors.shape[0]):- altered_translation_vectors[this_camera_number,:] = original_translation_vectors[this_camera_number,:] - camera_0_translation+ altered_translation_vectors[this_camera_number, :] = original_translation_vectors[this_camera_number,+ :] - camera_0_translation_anipose_camera_group_object.set_translations(altered_translation_vectors)logger.info(f"original translation vectors: {original_translation_vectors}")logger.info(f"altered translation vectors: {_anipose_camera_group_object.get_translations()}")return _anipose_camera_group_object+if __name__ == "__main__":passdiff --git a/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py b/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.pyindex 6e0783dc..3942bb1a 100644--- a/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py+++ b/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py@@ -129,7 +129,7 @@ class CalibrationPipelineOrchestrator:charuco_square_size: Union[int, float] = 1,):anipose_camera_calibrator = AniposeCameraCalibrator(self.session_id,- self._charuco_board_detector.charuco_board_object,+ self._charuco_board_detector.charuco_board_data_class_object,charuco_square_size=charuco_square_size)return anipose_camera_calibrator.calibrate_camera_capture_volume()diff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.py b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.pyindex 944e1e00..31f1c412 100644--- a/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.py@@ -3,7 +3,7 @@ import loggingimport cv2from src.cameras.capture.dataclasses.frame_payload import FramePayload-from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoard+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoardDataClassfrom src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_frame_payload import \CharucoFramePayloadfrom src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData@@ -17,10 +17,10 @@ logger = logging.getLogger(__name__)class CharucoBoardDetector:def __init__(self):- self.charuco_board_object = CharucoBoard()- self.charuco_board = self.charuco_board_object.charuco_board- self.aruco_marker_dict = self.charuco_board_object.aruco_marker_dict- self.number_of_charuco_corners = self.charuco_board_object.number_of_charuco_corners+ self.charuco_board_data_class_object = CharucoBoardDataClass()+ self.cv2_aruco_charuco_board = self.charuco_board_data_class_object.charuco_board+ self.aruco_marker_dict = self.charuco_board_data_class_object.aruco_marker_dict+ self.number_of_charuco_corners = self.charuco_board_data_class_object.number_of_charuco_cornersdef detect_charuco_board(self, raw_frame_payload: FramePayload) -> CharucoFramePayload:@@ -83,7 +83,7 @@ class CharucoBoardDetector:results = cv2.aruco.interpolateCornersCharuco(aruco_square_corners,aruco_square_ids,grayscale_image,- self.charuco_board+ self.cv2_aruco_charuco_board)if results[1] is not None and results[2] is not None and len(results[1]) > 3:@@ -95,7 +95,8 @@ class CharucoBoardDetector:if len(charuco_ids) == self.number_of_charuco_corners:full_board_found = True- return CharucoViewData(full_board_found=full_board_found,+ return CharucoViewData(charuco_board_object=self.charuco_board_data_class_object,+ full_board_found=full_board_found,some_charuco_corners_found=some_charuco_corners_found,any_markers_found=any_markers_found,charuco_corners=charuco_corners,diff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.pyindex 60511d6f..1a88ec0e 100644--- a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.py@@ -6,7 +6,7 @@ import numpy as np@dataclass-class CharucoBoard:+class CharucoBoardDataClass:aruco_marker_dict: Dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)number_of_squares_width: int = 7number_of_squares_height: int = 5diff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.pyindex 45ef0e18..46b7db8d 100644--- a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.py@@ -1,9 +1,16 @@+import loggingfrom dataclasses import dataclassimport numpy as np+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoardDataClass++logger = logging.getLogger(__name__)++@dataclassclass CharucoViewData:+ charuco_board_object: CharucoBoardDataClass = None # the charuco board definition this 'view' was trying to detectcharuco_corners: np.ndarray = Nonecharuco_ids: np.ndarray = Nonearuco_square_corners: np.ndarray = None@@ -14,4 +21,9 @@ class CharucoViewData:image_width: int = Noneimage_height: int = None-+ def __post_init__(self):+ if self.some_charuco_corners_found:+ if self.some_charuco_corners_found:+ if self.charuco_board_object is None:+ logger.warning(+ 'If there is any data in this view point, you should also include a copy of the charcuo_board that was being detected')diff --git a/src/pipelines/session_pipeline/data_classes/data_3d_single_frame_payload.py b/src/pipelines/session_pipeline/data_classes/data_3d_single_frame_payload.pynew file mode 100644index 00000000..0b859770--- /dev/null+++ b/src/pipelines/session_pipeline/data_classes/data_3d_single_frame_payload.py@@ -0,0 +1,10 @@+import numpy as np+from pydantic import BaseModel++class TweakedModel(BaseModel):+ class Config:+ arbitrary_types_allowed = True++class Data3dSingleFramePayload(TweakedModel):+ data3d_trackedPointNum_xyz: np.ndarray = None+ data3d_trackedPointNum_reprojectionError: np.ndarray = Nonediff --git a/src/pipelines/session_pipeline/session_pipeline_orchestrator.py b/src/pipelines/session_pipeline/session_pipeline_orchestrator.pyindex f5a3b6b0..7a9038ef 100644--- a/src/pipelines/session_pipeline/session_pipeline_orchestrator.py+++ b/src/pipelines/session_pipeline/session_pipeline_orchestrator.py@@ -2,7 +2,7 @@ import timeimport loggingimport tracebackfrom pathlib import Path-from typing import Dict+from typing import Dict, Unionimport cv2import numpy as np@@ -17,6 +17,8 @@ from src.core_processor.mediapipe_skeleton_detection.mediapipe_skeleton_detectiofrom src.core_processor.show_cam_window import show_cam_windowfrom src.core_processor.utils.image_fps_writer import write_fps_to_imagefrom src.pipelines.calibration_pipeline.calibration_pipeline_orchestrator import CalibrationPipelineOrchestrator++from src.pipelines.session_pipeline.data_classes.data_3d_single_frame_payload import Data3dSingleFramePayloadfrom src.qt_visualizer_and_gui.qt_visualizer_and_gui import QTVisualizerAndGuilogger = logging.getLogger(__name__)@@ -98,7 +100,7 @@ class SessionPipelineOrchestrator:returnelif load_calibration_from_session_id is not None:calibration_orchestrator = CalibrationPipelineOrchestrator(load_calibration_from_session_id)- else:+ else: # create new calibrationcalibration_orchestrator = CalibrationPipelineOrchestrator(self.session_id)calibration_orchestrator.record_videos(show_visualizer_gui=False,save_video_in_frame_loop=False,@@ -108,7 +110,7 @@ class SessionPipelineOrchestrator:self._anipose_camera_calibration_object = calibration_orchestrator.run_anipose_camera_calibration(charuco_square_size=39)- def run_big_frame_loop(+ def run_big_old_bongo_frame_loop(self,show_camera_views_in_windows=True,show_visualizer_gui=True,@@ -117,10 +119,10 @@ class SessionPipelineOrchestrator:reconstruct_3d=True,save_video=True,):- self._multi_frames_processed =0+ self._multi_frames_processed = 0with self._open_cv_camera_manager.start_capture_session_all_cams() as connected_cameras_dict:-+ self._number_of_cameras = len(connected_cameras_dict)if detect_charuco:incoming_charuco_frame_data_per_camera_dict = {}for this_webcam_id in connected_cameras_dict.keys():@@ -138,12 +140,12 @@ class SessionPipelineOrchestrator:timestamp_manager.increment_main_loop_timestamp_logger(time.perf_counter_ns())- #reconstruct 3d data, if there's enough new data to so+ # reconstruct 3d data, if there's enough new data to soif reconstruct_3d:if detect_charuco:- self._reconstruct_3d_charuco(incoming_charuco_frame_data_per_camera_dict)--+ charuco_frame_payload = self._reconstruct_3d_charuco(incoming_charuco_frame_data_per_camera_dict)+ if show_visualizer_gui:+ self._visualizer_gui.update_3d_view_port(charuco_frame_payload, type='charuco')for this_webcam_id, this_open_cv_camera in connected_cameras_dict.items():if not this_open_cv_camera.new_frame_ready:@@ -209,7 +211,9 @@ class SessionPipelineOrchestrator:if show_visualizer_gui:self._visualizer_gui.close()- def _reconstruct_3d_charuco(self, incoming_charuco_frame_data_per_camera_dict):+ def _reconstruct_3d_charuco(self, incoming_charuco_frame_data_per_camera_dict) -> \+ Union[None, Data3dSingleFramePayload]:+next_multi_frame_number = self._multi_frames_processednew_multi_frame_ready = []@@ -219,22 +223,82 @@ class SessionPipelineOrchestrator:if not all(new_multi_frame_ready):return- #new multiframe!- self._multi_frames_processed+=1+ # new multiframe!+ self._multi_frames_processed += 1- this_multi_frame_charuco_data=[]+ this_multi_frame_charuco_data = []this_multi_frame_timestamps = []+ each_cam_number_of_frames = []+ any_charuco_data = []for this_webcam_id in incoming_charuco_frame_data_per_camera_dict.keys():this_cam_data_list = incoming_charuco_frame_data_per_camera_dict[this_webcam_id]+ each_cam_number_of_frames.append(len(this_cam_data_list))this_cam_this_frame = this_cam_data_list[next_multi_frame_number]this_multi_frame_timestamps.append(this_cam_this_frame.raw_frame_payload.timestamp)this_multi_frame_charuco_data.append(this_cam_this_frame)+ any_charuco_data.append(this_cam_this_frame.charuco_view_data.some_charuco_corners_found)timestamp_min = np.min(this_multi_frame_timestamps)timestamp_max = np.max(this_multi_frame_timestamps)- this_multiframe_timestamp_interval_sec = (timestamp_max - timestamp_min)/1e9+ this_multiframe_timestamp_interval_sec = (timestamp_max - timestamp_min) / 1e9self._multi_frame_timestamp_intervals_sec.append(this_multiframe_timestamp_interval_sec)+ print(f"TODO:sync with timestamps, not frame index - multi_frame#: {self._multi_frames_processed - 1},"+ f" each_cam_#_of_frames: {each_cam_number_of_frames}, "+ f" intra_multi_frame_timestamp_range: {this_multiframe_timestamp_interval_sec:.3f}", )++ min_cameras_to_reconstruct = 2+ if sum(any_charuco_data) > min_cameras_to_reconstruct: # if at least 2 cameras have any data, then we can try to make some 3d dottos+ charuco2d_data_per_cam_dict = self.format_charuco2d_data(this_multi_frame_charuco_data)+ charuco_3d_data_payload = self._triangulate_2d_data(charuco2d_data_per_cam_dict,+ self._charuco_board_detector.number_of_charuco_corners)+ return charuco_3d_data_payload++ def format_charuco2d_data(self, this_multi_frame_charuco_data) -> Dict:++ number_of_tracked_points = this_multi_frame_charuco_data[+ 0].charuco_view_data.charuco_board_object.number_of_charuco_corners++ charuco2d_data_per_cam_dict = {}+ base_charuco_data_npy_with_nans_for_missing_data_xy = np.zeros((number_of_tracked_points, 2))+ base_charuco_data_npy_with_nans_for_missing_data_xy[:] = np.nan+ for this_cam_data in this_multi_frame_charuco_data:+ this_frame_charuco_data_xy = base_charuco_data_npy_with_nans_for_missing_data_xy.copy()+ charuco_ids_in_this_frame_idx = this_cam_data.charuco_view_data.charuco_ids+ charuco_corners_in_this_frame_xy = this_cam_data.charuco_view_data.charuco_corners+ this_frame_charuco_data_xy[charuco_ids_in_this_frame_idx, :] = charuco_corners_in_this_frame_xy++ this_webcam_id = this_cam_data.raw_frame_payload.webcam_id+ charuco2d_data_per_cam_dict[this_webcam_id] = this_frame_charuco_data_xy++ return charuco2d_data_per_cam_dict++ def _triangulate_2d_data(self, data2d_per_cam_dict: Dict,+ number_of_tracked_points: int) -> Data3dSingleFramePayload:++ each_cam2d_data_list = [this_cam_charuco2d_frame_xy for this_cam_charuco2d_frame_xy in+ data2d_per_cam_dict.values()]+ data2d_trackedPointNum_xyz_camNum = np.dstack(+ each_cam2d_data_list) # stack lists (in depth) to make one numpy array with dimensions [number_of_tracked_points, XYZ, number_of_cameras]+ data2d_camNum_trackedPointNum_xyz = data2d_trackedPointNum_xyz_camNum.reshape(self._number_of_cameras,+ number_of_tracked_points,+ 2) # reshape to fit into anipose (new dimensions [number_of_cameras, number_of_tracked_points, XYZ]++ # THIS IS WHERE THE MAGIC HAPPENS - 2d data from calibrated, synchronized cameras has now become a 3d estimate. Hurray! :`D+ data3d_trackedPointNum_xyz = self._anipose_camera_calibration_object.triangulate(+ data2d_camNum_trackedPointNum_xyz,+ progress=True)++ # Reprojection error is a measure of the quality of the reconstruction. It is the distance (error) between the original 2d point and a reprojection of the 3d point back onto the image plane.+ # TODO - use this for filtering data (i.e. if one view has very high reprojection error, re-do the triangulation without that camera's view data)+ # TODO - we are currently using each tracking method's `confidence` values (or whatever they choose to call it) for thresholding, but it's problematic because of the neural_networks' propensity to apply high confidence to bonkers estimates, which leads to SPOOKY GHOST SKELETONS! :O+ data3d_trackedPointNum_reprojectionError = self._anipose_camera_calibration_object.reprojection_error(+ data3d_trackedPointNum_xyz,+ data2d_camNum_trackedPointNum_xyz,+ mean=True)++ return Data3dSingleFramePayload(data3d_trackedPointNum_xyz=data3d_trackedPointNum_xyz,+ data3d_trackedPointNum_reprojectionError=data3d_trackedPointNum_reprojectionError, )if __name__ == "__main__":@@ -242,9 +306,11 @@ if __name__ == "__main__":this_session_orchestrator = SessionPipelineOrchestrator()this_session_orchestrator.calibrate_camera_capture_volume(use_most_recent_calibration=True)- this_session_orchestrator.run_big_frame_loop(save_video=False,- show_visualizer_gui=False,- detect_mediapipe=False,- detect_charuco=True,- reconstruct_3d=True,- )+ # this_session_orchestrator.calibrate_camera_capture_volume()+ this_session_orchestrator.run_big_old_bongo_frame_loop(show_camera_views_in_windows=False,+ save_video=False,+ show_visualizer_gui=True,+ detect_mediapipe=False,+ detect_charuco=True,+ reconstruct_3d=True,+ )diff --git a/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py b/src/qt_visualizer_and_gui/qt_visualizer_and_gui.pyindex 6c1171f2..520399e1 100644--- a/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py+++ b/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py@@ -11,6 +11,7 @@ from pyqtgraph.dockarea.DockArea import DockAreafrom pyqtgraph.Qt import QtWidgetsfrom src.core_processor.fps.timestamp_manager import TimestampManager+from src.pipelines.session_pipeline.data_classes.data_3d_single_frame_payload import Data3dSingleFramePayloadlogger = logging.getLogger(__name__)