Skip to content

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.py
index 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.py
index 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.py
index 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_path
from src.pipelines.calibration_pipeline.anipose_camera_calibration import freemocap_anipose
from 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 \
+ CharucoBoardDataClass
logger = 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_id
self._charuco_board_object = charuco_board_object
if 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_size
self.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 files
calibration_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__":
pass
diff --git a/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py b/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py
index 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.py
index 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 logging
import cv2
from 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 CharucoBoardDataClass
from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_frame_payload import \
CharucoFramePayload
from 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_corners
def 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.py
index 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 = 7
number_of_squares_height: int = 5
diff --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.py
index 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 logging
from dataclasses import dataclass
import numpy as np
+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoardDataClass
+
+logger = logging.getLogger(__name__)
+
+
@dataclass
class CharucoViewData:
+ charuco_board_object: CharucoBoardDataClass = None # the charuco board definition this 'view' was trying to detect
charuco_corners: np.ndarray = None
charuco_ids: np.ndarray = None
aruco_square_corners: np.ndarray = None
@@ -14,4 +21,9 @@ class CharucoViewData:
image_width: int = None
image_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.py
new file mode 100644
index 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 = None
diff --git a/src/pipelines/session_pipeline/session_pipeline_orchestrator.py b/src/pipelines/session_pipeline/session_pipeline_orchestrator.py
index 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 time
import logging
import traceback
from pathlib import Path
-from typing import Dict
+from typing import Dict, Union
import cv2
import numpy as np
@@ -17,6 +17,8 @@ from src.core_processor.mediapipe_skeleton_detection.mediapipe_skeleton_detectio
from src.core_processor.show_cam_window import show_cam_window
from src.core_processor.utils.image_fps_writer import write_fps_to_image
from src.pipelines.calibration_pipeline.calibration_pipeline_orchestrator import CalibrationPipelineOrchestrator
+
+from src.pipelines.session_pipeline.data_classes.data_3d_single_frame_payload import Data3dSingleFramePayload
from src.qt_visualizer_and_gui.qt_visualizer_and_gui import QTVisualizerAndGui
logger = logging.getLogger(__name__)
@@ -98,7 +100,7 @@ class SessionPipelineOrchestrator:
return
elif load_calibration_from_session_id is not None:
calibration_orchestrator = CalibrationPipelineOrchestrator(load_calibration_from_session_id)
- else:
+ else: # create new calibration
calibration_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 = 0
with 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 so
if 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_processed
new_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) / 1e9
self._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.py
index 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 DockArea
from pyqtgraph.Qt import QtWidgets
from src.core_processor.fps.timestamp_manager import TimestampManager
+from src.pipelines.session_pipeline.data_classes.data_3d_single_frame_payload import Data3dSingleFramePayload
logger = logging.getLogger(__name__)