Skip to content

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.py
index 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' -> c
new_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, orientations
diff --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.py
index 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: