Free Motion Capture for Everyone πβ¨
TypeScript
bout ready to get into the REAL calibratin :O
jonmatthisfb42fd71dbbc
diff --git a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.pyindex d2c4809a..400d6d37 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_aggregation_node.py@@ -1,6 +1,5 @@import loggingimport multiprocessing-import threadingimport timefrom dataclasses import dataclassfrom multiprocessing import Queue, Process@@ -11,7 +10,7 @@ from skellycam import CameraIdfrom freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputDatafrom freemocap.pipelines.calibration_pipeline.multi_camera_calibrator import MultiCameraCalibrator-from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import SharedViewAccumulator+from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibratorfrom freemocap.pipelines.pipeline_abcs import BaseAggregationLayerOutputData, BasePipelineStageConfig, \BaseAggregationNode, BasePipelineOutputData@@ -67,10 +66,9 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):camera_id ininput_queues.keys()}- shared_view_accumulator: SharedViewAccumulator = SharedViewAccumulator.create(- camera_ids=list(input_queues.keys()))- multi_camera_calibrator: MultiCameraCalibrator | None = None+ multi_camera_calibrator = MultiCameraCalibrator.from_camera_ids(camera_ids=list(input_queues.keys()))+ camera_calibration_estimators: dict[CameraId, SingleCameraCalibrator]|None = Nonetry:while not shutdown_event.is_set():@@ -93,17 +91,13 @@ class CalibrationAggregationProcessNode(BaseAggregationNode):raise ValueError(f"Frame numbers from camera nodes do not match! got {frame_numbers}")multi_frame_number = frame_numbers.pop()- if multi_camera_calibrator is None:+ if not multi_camera_calibrator.has_calibration:+if multi_frame_number % 10 == 0:# Accumulate shared views- shared_view_accumulator.receive_camera_node_output(multi_frame_number=multi_frame_number,- camera_node_output=camera_node_incoming_data)- logger.trace(f"Shared view accumulator: {shared_view_accumulator.shared_view_count_by_camera}")- if shared_view_accumulator.all_cameras_have_min_shared_views(10) and multi_camera_calibrator is None:-- multi_camera_calibrator = MultiCameraCalibrator.initialize(- shared_charuco_views=shared_view_accumulator.shared_target_views,- calibrate_cameras=True)+ multi_camera_calibrator.receive_camera_node_output(multi_frame_number=multi_frame_number,+ camera_node_output_by_camera=camera_node_incoming_data)+ if multi_camera_calibrator.all_cameras_have_min_shared_views():multi_camera_calibrator.calibrate()else:logger.info("Do triangulation and stuff with the calibration data")diff --git a/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py b/freemocap/pipelines/calibration_pipeline/calibration_camera_node.pyindex 7c3f2cd9..e56673eb 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_camera_node.py@@ -81,24 +81,12 @@ class CalibrationCameraNode(BaseCameraNode):time_to_retrieve = time.perf_counter_ns() - tiktik = time.perf_counter_ns()observation, raw_results = charuco_tracker.process_image(frame.image, annotate_image=False)- if camera_calibration_estimator is None:- camera_calibration_estimator = SingleCameraCalibrator.create_initial(- camera_id=camera_id,- image_size=frame.image.shape[:2],- aruco_marker_ids=list(charuco_tracker.detector.board.getIds()),- aruco_corners_in_object_coordinates=charuco_tracker.aruco_corners_in_object_coordinates,- charuco_corner_ids=charuco_tracker.charuco_corner_ids,- charuco_corners_in_object_coordinates=charuco_tracker.charuco_corners_in_object_coordinates,- )- if not camera_calibration_estimator.has_calibration and not observation.charuco_empty and frame.frame_number % 10 == 0:- camera_calibration_estimator.add_observation(observation)- # if len(camera_calibration_estimator.charuco_observations) == 30:- # camera_calibration_estimator.update_calibration_estimate()++time_to_process = time.perf_counter_ns() - tikoutput_queue.put(CalibrationCameraNodeOutputData(frame_metadata=FrameMetadata.from_frame_metadata_array(frame.metadata),charuco_observation=observation,- calibration_estimate=camera_calibration_estimator.current_estimate,time_to_retrieve_frame_ns=time_to_retrieve,time_to_process_frame_ns=time_to_process),diff --git a/freemocap/pipelines/calibration_pipeline/calibration_camera_node_output_data.py b/freemocap/pipelines/calibration_pipeline/calibration_camera_node_output_data.pyindex fade3ad3..7f4160ce 100644--- a/freemocap/pipelines/calibration_pipeline/calibration_camera_node_output_data.py+++ b/freemocap/pipelines/calibration_pipeline/calibration_camera_node_output_data.py@@ -1,14 +1,12 @@from skellycam.core.frames.payloads.metadata.frame_metadata import FrameMetadatafrom skellytracker.trackers.charuco_tracker.charuco_observation import CharucoObservation-from freemocap.pipelines.calibration_pipeline.single_camera_calibration_estimate import SingleCameraCalibrationEstimatefrom freemocap.pipelines.pipeline_abcs import BaseCameraNodeOutputDataclass CalibrationCameraNodeOutputData(BaseCameraNodeOutputData):frame_metadata: FrameMetadatacharuco_observation: CharucoObservation- calibration_estimate: SingleCameraCalibrationEstimate | None = None@propertydef can_see_target(self) -> bool:diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.pyindex ac5fbfb2..4975d18e 100644--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibration/anipose_calibration_algorithm.py@@ -1,5 +1,4 @@import logging-from copy import deepcopyimport cv2import numpy as np@@ -8,19 +7,26 @@ from pydantic import BaseModelfrom scipy import optimizefrom scipy.linalg import inv as inversefrom scipy.sparse import dok_matrix+from skellycam import CameraIdfrom tqdm import trangefrom freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import PixelPoints2D, \- ObjectPoints3D, ExtrinsicsParameters, IntrinsicsParameters, ReprojectionError+ ObjectPoints3D, ExtrinsicsParameters, IntrinsicsParameters, ReprojectionError, PixelPoints2DByCamera, PointIds, \+ RotationVectorsByCamera, TranslationVectorsByCamerafrom freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_utilities import \calculate_error_bounds, transform_points, construct_camera_extrinsics_matrix, \get_rotation_and_translation_vector_from_extrinsics_matrix, get_error_dict-from freemocap.pipelines.calibration_pipeline.multi_camera_calibrator import MultiCameraCalibrationInputDatafrom freemocap.pipelines.calibration_pipeline.single_camera_calibration_estimate import SingleCameraCalibrationEstimatelogger = logging.getLogger(__name__)+class MultiCameraCalibrationInputData(BaseModel):+ pixel_points2d: PixelPoints2DByCamera+ object_points3d: ObjectPoints3D+ points_ids: PointIds+ rotation_vectors: RotationVectorsByCamera+ translation_vectors: TranslationVectorsByCamera@@ -102,6 +108,8 @@ class AniposeSingleCameraCalibrationHelper(BaseModel):class AniposeMultiCameraCalibrator(BaseModel):+ camera_helpers: dict[CameraId, AniposeSingleCameraCalibrationHelper]+max_iterations: int = 10starting_error_threshold: float = 15.0ending_error_threshold: float = 1.0@@ -111,24 +119,19 @@ class AniposeMultiCameraCalibrator(BaseModel):number_of_samples_full: int = 1000error_threshold: float = 0.3- def run_iterative_bundle_adjustment(- self,- calibration_input_data: MultiCameraCalibrationInputData,+ @classmethod+ def from_single_camera_calibrators(cls, single_camera_calibrators: dict[CameraId, SingleCameraCalibrationEstimate]):+ camera_helpers = {camera_id: AniposeSingleCameraCalibrationHelper(calibration_estimate=calibration_estimate)+ for camera_id, calibration_estimate in single_camera_calibrators.items()}+ return cls(camera_helpers=camera_helpers)- ) -> float:+ def run_iterative_bundle_adjustment(self) -> float:"""Performs iterative bundle adjustment to refine camera parameters.based on Anipose's iterative bundle adjustment method. (`bundle_adjust_iter`)"""- original_calibration_input_data = deepcopy(calibration_input_data)- original_points2d = original_calibration_input_data.pixel_points2d- # Validate input shapes- if not original_points2d.shape[0] == len(self.camera_calibration_estimates):- raise ValueError(- f"Invalid points shape, expected first dimension to be equal to "- f"number of cameras ({len(self.camera_calibration_estimates)}), but got shape {original_points2d.shape}"- )+ original_points2d = np.array([camera_helper.pixel_points2d for camera_helper in self.camera_helpers.values()])# Initializeerror_list = []@@ -142,13 +145,13 @@ class AniposeMultiCameraCalibrator(BaseModel):f"\n\tOriginal error: {original_error:.2f}")# Resample initial set of points to prioritize shared views- resampled_calibration_input_data = resample_points_based_on_shared_views(- original_calibration_input_data,- number_of_samples_to_return=self.number_of_samples_full- )- initial_error = self.get_mean_reprojection_error(resampled_calibration_input_data.pixel_points2d, median=True)+ # resampled_calibration_input_data = resample_points_based_on_shared_views(+ # original_calibration_input_data,+ # number_of_samples_to_return=self.number_of_samples_full+ # )+ # initial_error = self.get_mean_reprojection_error(resampled_calibration_input_data.pixel_points2d, median=True)- logger.debug("Initial error after resampling: ", initial_error)+ # logger.debug("Initial error after resampling: ", initial_error)# Calculate dynamic error thresholds for each iterationerror_thresholds = np.exp(@@ -218,11 +221,11 @@ class AniposeMultiCameraCalibrator(BaseModel):return float(final_error)- @jit(parallel=True, forceobj=True)+ # @jit(parallel=True, forceobj=True)def calculate_reprojection_error(self,points_3d: np.ndarray,points_2d: np.ndarray,- mean=False):+ mean:bool=False):"""Given an Nx3 array of 3D points and an CxNx2 array of 2D points,where N is the number of points and C is the number of cameras,this returns an CxNx2 array of errors.diff --git a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.pyindex 42fd8223..daa3b195 100644--- a/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py+++ b/freemocap/pipelines/calibration_pipeline/multi_camera_calibrator.py@@ -1,57 +1,93 @@+import logging+from pydantic import BaseModelfrom skellycam import CameraId-from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \- PixelPoints2DByCamera, ObjectPoints3D, PointIds, RotationVectorsByCamera, TranslationVectorsByCamera-from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import TargetViewByCamera-from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator-class MultiCameraCalibrationInputData(BaseModel):- pixel_points2d: PixelPoints2DByCamera- object_points3d: ObjectPoints3D- points_ids: PointIds- rotation_vectors: RotationVectorsByCamera- translation_vectors: TranslationVectorsByCamera+from freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputData+from freemocap.pipelines.calibration_pipeline.multi_camera_calibration.anipose_calibration_algorithm import \+ AniposeMultiCameraCalibrator+from freemocap.pipelines.calibration_pipeline.shared_view_accumulator import SharedViewAccumulator-import logginglogger = logging.getLogger(__name__)+from freemocap.pipelines.calibration_pipeline.single_camera_calibrator import SingleCameraCalibrator+++class MultiCameraCalibrationEstimate(BaseModel):+ pass++class MultiCameraCalibrator(BaseModel):principal_camera_id: CameraIdcamera_id_to_index: dict[CameraId, int]- single_camera_calibrators: dict[CameraId, SingleCameraCalibrator]+ shared_view_accumulator: SharedViewAccumulator++ single_camera_calibrators: dict[CameraId, SingleCameraCalibrator] | None = None+ multi_camera_calibration_estimate: MultiCameraCalibrationEstimate | None = None+ minimum_views_to_reconstruct: int | None = 20++ @property+ def has_calibration(self) -> bool:+ if self.single_camera_calibrators is None or self.multi_camera_calibration_estimate is None:+ return False+ return all(single_camera_calibrator.has_calibration for single_camera_calibrator in+ self.single_camera_calibrators.values()) and self.multi_camera_calibration_estimate.has_calibration@classmethod- def initialize(cls,- shared_charuco_views: list[TargetViewByCamera],- calibrate_cameras: bool = True):+ def from_camera_ids(cls, camera_ids: list[CameraId], principal_camera_id: CameraId | None = None):+ return cls(principal_camera_id=principal_camera_id if principal_camera_id is not None else min(camera_ids),+ camera_id_to_index={camera_id: index for index, camera_id in enumerate(camera_ids)},+ shared_view_accumulator=SharedViewAccumulator.create(camera_ids=camera_ids),+ )++ def receive_camera_node_output(self, multi_frame_number: int,+ camera_node_output_by_camera: dict[CameraId, CalibrationCameraNodeOutputData]):++ self.add_observations_to_single_camera_calibrators(camera_node_output_by_camera, multi_frame_number)++ self.shared_view_accumulator.receive_camera_node_output(multi_frame_number=multi_frame_number,+ camera_node_output_by_camera=camera_node_output_by_camera)- # Calculate the calibration estimate for each camera+ logger.trace(f"Shared view accumulator: {self.shared_view_accumulator.shared_view_count_by_camera}")- charuco_view_by_camera = {}+ def add_observations_to_single_camera_calibrators(self, camera_node_output_by_camera, multi_frame_number):+ if self.single_camera_calibrators is None:+ self.initialize_single_camera_calibrators()+ for camera_id, output in camera_node_output_by_camera.items():+ self.single_camera_calibrators[camera_id].add_observation(observation=output.charuco_observation)- for target_view_by_camera in shared_charuco_views:- for camera_id, camera_node_output in target_view_by_camera.views_by_camera.items():- if camera_id not in charuco_view_by_camera:- charuco_view_by_camera[camera_id] = []- charuco_view_by_camera[camera_id].append(camera_node_output)+ def initialize_single_camera_calibrators(self):+ if self.latest_camera_node_output_by_camera is None:+ raise ValueError("No camera node output data to initialize single camera calibrators with!")+ self.single_camera_calibrators = {}+ for camera_id, node_output in self.latest_camera_node_output_by_camera.items():+ self.single_camera_calibrators[camera_id] = SingleCameraCalibrator.create_initial(+ camera_id=camera_id,+ image_size=node_output.charuco_observation.image_size,+ all_aruco_marker_ids=node_output.charuco_observation.all_aruco_ids,+ all_aruco_corners_in_object_coordinates=node_output.charuco_observation.all_aruco_corners_in_object_coordinates,+ all_charuco_corner_ids=node_output.charuco_observation.all_charuco_ids,+ all_charuco_corners_in_object_coordinates=node_output.charuco_observation.all_charuco_corners_in_object_coordinates+ )- # Find the principal camera (usually Camera0, but this finds the lowest indexed camera)- principal_camera_id = min(key for key in charuco_view_by_camera.keys())+ def all_cameras_have_min_shared_views(self, min_shared_views: int | None = None) -> bool:+ return self.shared_view_accumulator.all_cameras_have_min_shared_views(+ min_shared_views=self.minimum_views_to_reconstruct if min_shared_views is None else min_shared_views)- single_camera_calibrators = {}- for camera_id, camera_node_outputs in charuco_view_by_camera.items():- single_camera_calibrators[camera_id] = SingleCameraCalibrator.from_camera_node_outputs(- camera_node_outputs=camera_node_outputs,- calibrate_camera=calibrate_cameras)+ logger.trace(+ f"Calibrated camera {camera_id}:\n{self.single_camera_calibrators[camera_id].current_estimate.model_dump_json(indent=2)}")+ def calibrate(self) -> MultiCameraCalibrationEstimate:+ logger.debug(+ f"Calibrating multi-camera system for cameras: {self.single_camera_calibrators.keys()} with principal camera: {self.principal_camera_id} and {len(self.shared_view_accumulator.shared_target_views)} shared views")+ self.update_single_camera_calibration_estimates()+ self.run_multi_camera_optimization()+ logger.success(+ f"Multi-camera calibration complete! \n {self.multi_camera_calibration_estimate.model_dump_json(indent=2)}")+ return self.multi_camera_calibration_estimate- return cls(principal_camera_id=principal_camera_id,- single_camera_calibrators=single_camera_calibrators,- camera_id_to_index={camera_id: index for index, camera_id in enumerate(single_camera_calibrators.keys())}- )+ def run_multi_camera_optimization(self):+ self.multi_camera_calibration_estimate = AniposeMultiCameraCalibrator.from_single_camera_calibrators(+ single_camera_calibrators=self.single_camera_calibrators).run_iterative_bundle_adjustment()- def calibrate(self):- for single_camera_calibrator in self.single_camera_calibrators.values():- single_camera_calibrator.update_calibration_estimate()- logger.trace(f"Calibrated camera {single_camera_calibrator.camera_id}:\n{single_camera_calibrator.current_estimate.model_dump_json(indent=2)}")- f=9\ No newline at end of file+ return self.multi_camera_calibration_estimatediff --git a/freemocap/pipelines/calibration_pipeline/positional_6dof.py b/freemocap/pipelines/calibration_pipeline/positional_6dof.pyindex f5606b23..93a3ae9d 100644--- a/freemocap/pipelines/calibration_pipeline/positional_6dof.py+++ b/freemocap/pipelines/calibration_pipeline/positional_6dof.py@@ -1,10 +1,11 @@+import cv2import numpy as npfrom numpydantic import NDArray, Shapefrom pydantic import BaseModelfrom freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \TransformationMatrix, TranslationVector, RotationVector-import cv2+class Positional6DoF(BaseModel):translation: TranslationVector= np.zeros(3)diff --git a/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py b/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.pyindex 0cd05b7b..93fa70b4 100644--- a/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py+++ b/freemocap/pipelines/calibration_pipeline/shared_view_accumulator.py@@ -1,23 +1,24 @@-from typing import Dict-from pydantic import BaseModel, Fieldfrom skellycam import CameraIdfrom freemocap.pipelines.calibration_pipeline.calibration_camera_node_output_data import CalibrationCameraNodeOutputDataMultiFrameNumber = int-class TargetViewByCamera(BaseModel):+class MultiFrameTargetViews(BaseModel):multi_frame_number: MultiFrameNumber- views_by_camera: dict[CameraId, CalibrationCameraNodeOutputData] = Field(description="Key: CameraId of cameras that can see the target, Value: CameraNodeOutputData from that camera")+ camera_node_output_by_camera: dict[CameraId, CalibrationCameraNodeOutputData] = Field(description="Key: CameraId of cameras that can see the target, Value: CameraNodeOutputData from that camera")@propertydef cameras_that_can_see_target(self) -> list[CameraId]:- return [camera_id for camera_id, camera_node_output in self.views_by_camera.items() if camera_node_output.can_see_target]+ return [camera_id for camera_id, camera_node_output in self.camera_node_output_by_camera.items() if camera_node_output.can_see_target]@propertydef multiple_cameras_can_see_target(self) -> bool:return len(self.cameras_that_can_see_target) > 1+ @property+ def number_of_cameras_that_can_see_target(self) -> int:+ return len(self.cameras_that_can_see_target)class SharedViewAccumulator(BaseModel):"""@@ -25,32 +26,32 @@ class SharedViewAccumulator(BaseModel):and counts the number of shared views each camera has with each other camera (i.e. frames where both cameras can see the target)"""camera_ids: list[CameraId]- shared_views_by_frame: dict[MultiFrameNumber, TargetViewByCamera]+ target_views_by_frame: dict[MultiFrameNumber, MultiFrameTargetViews]@classmethoddef create(cls, camera_ids: list[CameraId]):- return cls(camera_ids=camera_ids, shared_views_by_frame={})+ return cls(camera_ids=camera_ids, target_views_by_frame={})@propertydef shared_view_count_by_camera(self) -> dict[CameraId, int]:shared_view_count_by_camera = {camera_id: 0 for camera_id in self.camera_ids}- for shared_view_by_frame in self.shared_views_by_frame.values():+ for shared_view_by_frame in self.target_views_by_frame.values():if shared_view_by_frame.multiple_cameras_can_see_target:for camera_id in shared_view_by_frame.cameras_that_can_see_target:shared_view_count_by_camera[camera_id] += 1return shared_view_count_by_camera@property- def shared_target_views(self) -> list[TargetViewByCamera]:- return [shared_view_by_frame for shared_view_by_frame in self.shared_views_by_frame.values() if shared_view_by_frame.multiple_cameras_can_see_target]+ def shared_target_views(self) -> list[MultiFrameTargetViews]:+ return [shared_view_by_frame for shared_view_by_frame in self.target_views_by_frame.values() if shared_view_by_frame.multiple_cameras_can_see_target]def receive_camera_node_output(self, multi_frame_number: int,- camera_node_output: dict[CameraId, CalibrationCameraNodeOutputData]):- cameras_that_can_see_target = {camera_id: camera_node_output for camera_id, camera_node_output in camera_node_output.items() if camera_node_output.can_see_target}- self.shared_views_by_frame[multi_frame_number] = TargetViewByCamera(multi_frame_number=multi_frame_number, views_by_camera=cameras_that_can_see_target)+ camera_node_output_by_camera: dict[CameraId, CalibrationCameraNodeOutputData]):+ cameras_that_can_see_target = {camera_id: camera_node_output for camera_id, camera_node_output in camera_node_output_by_camera.items() if camera_node_output.can_see_target}+ self.target_views_by_frame[multi_frame_number] = MultiFrameTargetViews(multi_frame_number=multi_frame_number, camera_node_output_by_camera=cameras_that_can_see_target)- def all_cameras_have_min_shared_views(self, min_shared_views: int) -> bool:+ def all_cameras_have_min_shared_views(self, min_shared_views: int = 10) -> bool:return all(shared_views >= min_shared_views for shared_views in self.shared_view_count_by_camera.values())diff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.pyindex 301b8d0d..ff241960 100644--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibration_estimate.py@@ -1,13 +1,11 @@-import numpy as np-from numpydantic import NDArray, Shapefrom pydantic import BaseModel-from pydantic import Field+from skellycam import CameraIdfrom freemocap.pipelines.calibration_pipeline.multi_camera_calibration.calibration_numpy_types import \TransformationMatrix, CameraMatrix, CameraDistortionCoefficientsfrom freemocap.pipelines.calibration_pipeline.positional_6dof import Positional6DoF-from skellycam import CameraId+class SingleCameraCalibrationEstimate(BaseModel):camera_id: CameraIddiff --git a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.pyindex 4fafbe83..7f5bddfe 100644--- a/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py+++ b/freemocap/pipelines/calibration_pipeline/single_camera_calibrator.py@@ -31,7 +31,7 @@ class SingleCameraCalibrator(BaseModel):"""camera_id: CameraIdimage_size: tuple[int, ...]- object_points_views: list[NDArray[Shape["*, 3"], np.float32]]+ object_points_views: list[NDArray[Shape["* , 3"], np.float32]]image_points_views: list[NDArray[Shape["*, 2"], np.float32]]rotation_vectors: list[NDArray[Shape["*, 3"], np.float32]]translation_vectors: list[NDArray[Shape["*, 3"], np.float32]]@@ -74,10 +74,10 @@ class SingleCameraCalibrator(BaseModel):instance = cls.create_initial(camera_id=camera_id,image_size=observation.image_size,- aruco_marker_ids=observation.all_aruco_ids,- aruco_corners_in_object_coordinates=observation.all_aruco_corners_in_object_coordinates,- charuco_corner_ids=observation.all_charuco_ids,- charuco_corners_in_object_coordinates=observation.all_charuco_corners_in_object_coordinates,+ all_aruco_marker_ids=observation.all_aruco_ids,+ all_aruco_corners_in_object_coordinates=observation.all_aruco_corners_in_object_coordinates,+ all_charuco_corner_ids=observation.all_charuco_ids,+ all_charuco_corners_in_object_coordinates=observation.all_charuco_corners_in_object_coordinates,)for camera_node_outputs in camera_node_outputs:if camera_node_outputs.camera_id != camera_id:@@ -93,10 +93,10 @@ class SingleCameraCalibrator(BaseModel):def create_initial(cls,camera_id: CameraId,image_size: tuple[int, ...],- aruco_marker_ids: list[int],- aruco_corners_in_object_coordinates: list[np.ndarray[..., 3]],- charuco_corner_ids: list[int],- charuco_corners_in_object_coordinates: np.ndarray[..., 3],+ all_aruco_marker_ids: list[int],+ all_aruco_corners_in_object_coordinates: list[np.ndarray[..., 3]],+ all_charuco_corner_ids: list[int],+ all_charuco_corners_in_object_coordinates: np.ndarray[..., 3],number_of_distortion_coefficients: int = DEFAULT_INTRINSICS_COEFFICIENTS_COUNT):camera_matrix = np.eye(3)camera_matrix[0, 2] = image_size[0] / 2 # x_center@@ -105,17 +105,17 @@ class SingleCameraCalibrator(BaseModel):if not number_of_distortion_coefficients in [4, 5, 8, 12, 14]:raise ValueError("Invalid number of distortion coefficients. Must be 4, 5, 8, 12, or 14.")- if len(charuco_corner_ids) != charuco_corners_in_object_coordinates.shape[0]:+ if len(all_charuco_corner_ids) != all_charuco_corners_in_object_coordinates.shape[0]:raise ValueError("Number of charuco corner IDs must match the number of charuco corners.")- if len(aruco_marker_ids) != len(aruco_corners_in_object_coordinates):+ if len(all_aruco_marker_ids) != len(all_aruco_corners_in_object_coordinates):raise ValueError("Number of aruco marker IDs must match the number of aruco corners.")return cls(camera_id=camera_id,image_size=image_size,- charuco_corner_ids=charuco_corner_ids,- charuco_corners_in_object_coordinates=charuco_corners_in_object_coordinates,- aruco_marker_ids=aruco_marker_ids,- aruco_corners_in_object_coordinates=aruco_corners_in_object_coordinates,+ charuco_corner_ids=all_charuco_corner_ids,+ charuco_corners_in_object_coordinates=all_charuco_corners_in_object_coordinates,+ aruco_marker_ids=all_aruco_marker_ids,+ aruco_corners_in_object_coordinates=all_aruco_corners_in_object_coordinates,camera_matrix=camera_matrix,distortion_coefficients=np.zeros(number_of_distortion_coefficients),object_points_views=[],