Free Motion Capture for Everyone πβ¨
TypeScript
Update anipose_camera_calibrator.py (#723)
Aaron Cherian5db2fa3c4753
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.pyindex eda68af6..31c47f35 100644--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/anipose_camera_calibrator.py@@ -110,6 +110,9 @@ class AniposeCameraCalibrator:self._anipose_camera_group_object, groundplane_success = self.set_charuco_board_as_groundplane(self._anipose_camera_group_object)if groundplane_success.success:self._anipose_camera_group_object.metadata["groundplane_calibration"] = True++ self.get_real_world_matrices(self._anipose_camera_group_object)+calibration_toml_filename = create_camera_calibration_file_name(recording_name=self._calibration_videos_folder_path.parent.stem)@@ -223,7 +226,6 @@ class AniposeCameraCalibrator:logger.info("Getting 3d Charuco data")charuco_3d_flat = self._anipose_camera_group_object.triangulate(charuco_2d_flat)-charuco_3d_xyz = charuco_3d_flat.reshape(num_frames, num_tracked_points, 3)logger.info(f"Charuco 3d data reconstructed with shape: {charuco_3d_xyz.shape}")@@ -289,4 +291,24 @@ class AniposeCameraCalibrator:new_rmat = rmat_world_to_cam_i @ rmat_charuco_to_world #composes w' -> w (rotation matrix) with w -> c (rmat) to get w' -> cnew_rvec, _ = cv2.Rodrigues(new_rmat)rvecs_new[i] = new_rvec.flatten()- return Extrinsics(rvecs_new, tvecs_new)\ No newline at end of file+ return Extrinsics(rvecs_new, tvecs_new)++ def get_real_world_matrices(self,+ cam_group:freemocap_anipose.CameraGroup):+ rvecs = cam_group.get_rotations()+ tvecs = cam_group.get_translations()++ positions = []+ orientations = []+ for i in range(tvecs.shape[0]):+ rmat_world_to_cam_i, _ = cv2.Rodrigues(rvecs[i])++ rmat_cam_to_world = rmat_world_to_cam_i.T+ t_world = -rmat_cam_to_world @ tvecs[i]+ positions.append(t_world.astype(float).tolist())+ orientations.append(rmat_cam_to_world.astype(float).tolist())++ cam_group.set_world_positions(positions)+ cam_group.set_world_orientations(orientations)++ return positions, orientationsdiff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.pyindex eeafa6df..ce55c1c1 100644--- a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py@@ -402,6 +402,8 @@ class Camera:size=None,rvec=np.zeros(3),tvec=np.zeros(3),+ world_orientation = np.eye(3),+ world_position = np.zeros(3),name=None,extra_dist=False,):@@ -410,6 +412,8 @@ class Camera:self.set_size(size)self.set_rotation(rvec)self.set_translation(tvec)+ self.set_world_orientation(world_orientation)+ self.set_world_position(world_position)self.set_name(name)self.extra_dist = extra_dist@@ -421,6 +425,8 @@ class Camera:"distortions": self.get_distortions().tolist(),"rotation": self.get_rotation().tolist(),"translation": self.get_translation().tolist(),+ "world_orientation": self.get_world_orientation().tolist(),+ "world_position": self.get_world_position().tolist()}def load_dict(self, d):@@ -431,6 +437,9 @@ class Camera:self.set_name(d["name"])self.set_size(d["size"])+ self.set_world_orientation(d.get("world_orientation", np.eye(3)))+ self.set_world_position(d.get("world_position", np.zeros(3)))+def from_dict(d):cam = Camera()cam.load_dict(d)@@ -473,7 +482,19 @@ class Camera:def get_translation(self):return self.tvec-++ def set_world_orientation(self, world_orientation):+ self.world_orientation = np.asarray(world_orientation, dtype="float64").reshape(3, 3)++ def get_world_orientation(self):+ return self.world_orientation++ def set_world_position(self, world_position):+ self.world_position = np.array(world_position, dtype="float64").ravel()++ def get_world_position(self):+ return self.world_position+def get_extrinsics_mat(self):return make_M(self.rvec, self.tvec)@@ -567,6 +588,8 @@ class Camera:tvec=self.get_translation().copy(),name=self.get_name(),extra_dist=self.extra_dist,+ world_orientation=self.get_world_orientation().copy(),+ world_position=self.get_world_position().copy())@@ -1782,7 +1805,7 @@ class CameraGroup:cameras = [cam.copy() for cam in self.cameras]metadata = copy(self.metadata)return CameraGroup(cameras, metadata)-+def set_rotations(self, rvecs):for cam, rvec in zip(self.cameras, rvecs):cam.set_rotation(rvec)@@ -1791,6 +1814,20 @@ class CameraGroup:for cam, tvec in zip(self.cameras, tvecs):cam.set_translation(tvec)+ def set_world_positions(self, positions):+ for cam, position, in zip(self.cameras, positions):+ cam.set_world_position(position)++ def set_world_orientations(self, orientations):+ for cam, orientation in zip(self.cameras, orientations):+ cam.set_world_orientation(orientation)++ def get_world_positions(self):+ return np.stack([cam.get_world_position() for cam in self.cameras])++ def get_world_orientations(self):+ return np.stack([cam.get_world_orientation() for cam in self.cameras])+def get_rotations(self):rvecs = []for cam in self.cameras: