def test_so3_cos_angle(self, batch_size: int = 100):
"""
Check that `so3_relative_angle(R1, R2, cos_angle=False).cos()`
is the same as `so3_relative_angle(R1, R2, cos_angle=True)`
batches of randomly generated rotation matrices `R1` and `R2`.
"""
rot1 = TestSO3.init_rot(batch_size=batch_size)
rot2 = TestSO3.init_rot(batch_size=batch_size)
angles = so3_relative_angle(rot1, rot2, cos_angle=False).cos()
angles_ = so3_relative_angle(rot1, rot2, cos_angle=True)
self.assertTrue(torch.allclose(angles, angles_))
def test_so3_exp_to_log_to_exp(self, batch_size: int = 100):
"""
Check that `so3_exponential_map(so3_log_map(R))==R` for
a batch of randomly generated rotation matrices `R`.
"""
rot = TestSO3.init_rot(batch_size=batch_size)
rot_ = so3_exponential_map(so3_log_map(rot, eps=1e-8), eps=1e-8)
angles = so3_relative_angle(rot, rot_)
# TODO: a lot of precision lost here ...
self.assertClose(angles, torch.zeros_like(angles), atol=0.1)
def test_so3_exp_to_log_to_exp(self, batch_size: int = 100):
"""
Check that `so3_exponential_map(so3_log_map(R))==R` for
a batch of randomly generated rotation matrices `R`.
"""
rot = TestSO3.init_rot(batch_size=batch_size)
rot_ = so3_exponential_map(so3_log_map(rot))
angles = so3_relative_angle(rot, rot_)
max_angle = angles.max()
# a lot of precision lost here :(
# TODO: fix this test??
self.assertTrue(np.allclose(float(max_angle), 0.0, atol=0.1))
def test_so3_exp_to_log_to_exp(self, batch_size: int = 100):
"""
Check that `so3_exp_map(so3_log_map(R))==R` for
a batch of randomly generated rotation matrices `R`.
"""
rot = TestSO3.init_rot(batch_size=batch_size)
non_singular = (so3_rotation_angle(rot) - math.pi).abs() > 1e-2
rot = rot[non_singular]
rot_ = so3_exp_map(so3_log_map(rot, eps=1e-8, cos_bound=1e-8),
eps=1e-8)
self.assertClose(rot_, rot, atol=0.1)
angles = so3_relative_angle(rot, rot_, cos_bound=1e-4)
self.assertClose(angles, torch.zeros_like(angles), atol=0.1)
def calc_camera_distance(cam_1, cam_2):
"""
Calculates the divergence of a batch of pairs of cameras cam_1, cam_2.
The distance is composed of the cosine of the relative angle between
the rotation components of the camera extrinsics and the l2 distance
between the translation vectors.
"""
# rotation distance
R_distance = (
1. - so3_relative_angle(cam_1.R, cam_2.R, cos_angle=True)).mean()
# translation distance
T_distance = ((cam_1.T - cam_2.T)**2).sum(1).mean()
# the final distance is the sum
return R_distance + T_distance
def test_so3_log_to_exp_to_log_to_exp(self, batch_size: int = 100):
"""
Check that
`so3_exp_map(so3_log_map(so3_exp_map(log_rot)))
== so3_exp_map(log_rot)`
for a randomly generated batch of rotation matrix logarithms `log_rot`.
Unlike `test_so3_log_to_exp_to_log`, this test checks the
correctness of converting a `log_rot` which contains values > math.pi.
"""
log_rot = 2.0 * TestSO3.init_log_rot(batch_size=batch_size)
# check also the singular cases where rot. angle = {0, 2pi}
log_rot[:2] = 0
log_rot[1, 0] = 2.0 * math.pi - 1e-6
rot = so3_exp_map(log_rot, eps=1e-4)
rot_ = so3_exp_map(so3_log_map(rot, eps=1e-4, cos_bound=1e-6),
eps=1e-6)
self.assertClose(rot, rot_, atol=0.01)
angles = so3_relative_angle(rot, rot_, cos_bound=1e-6)
self.assertClose(angles, torch.zeros_like(angles), atol=0.01)
def test_so3_log_to_exp_to_log_to_exp(self, batch_size: int = 100):
"""
Check that
`so3_exponential_map(so3_log_map(so3_exponential_map(log_rot)))
== so3_exponential_map(log_rot)`
for a randomly generated batch of rotation matrix logarithms `log_rot`.
Unlike `test_so3_log_to_exp_to_log`, this test allows to check the
correctness of converting `log_rot` which contains values > math.pi.
"""
log_rot = 2.0 * TestSO3.init_log_rot(batch_size=batch_size)
# check also the singular cases where rot. angle = {0, pi, 2pi, 3pi}
log_rot[:3] = 0
log_rot[1, 0] = math.pi
log_rot[2, 0] = 2.0 * math.pi
log_rot[3, 0] = 3.0 * math.pi
rot = so3_exponential_map(log_rot, eps=1e-8)
rot_ = so3_exponential_map(so3_log_map(rot, eps=1e-8), eps=1e-8)
angles = so3_relative_angle(rot, rot_)
self.assertClose(angles, torch.zeros_like(angles), atol=0.01)
def _corresponding_cameras_alignment_test_case(
self,
cameras,
R_align_gt,
T_align_gt,
s_align_gt,
estimate_scale,
mode,
add_noise,
):
batch_size = cameras.R.shape[0]
# get target camera centers
R_new = torch.bmm(R_align_gt[None].expand_as(cameras.R), cameras.R)
T_new = (
torch.bmm(T_align_gt[None, None].repeat(batch_size, 1, 1), cameras.R)[:, 0]
+ cameras.T
) * s_align_gt
if add_noise != 0.0:
R_new = torch.bmm(
R_new, so3_exponential_map(torch.randn_like(T_new) * add_noise)
)
T_new += torch.randn_like(T_new) * add_noise
# create new cameras from R_new and T_new
cameras_tgt = cameras.clone()
cameras_tgt.R = R_new
cameras_tgt.T = T_new
# align cameras and cameras_tgt
cameras_aligned = corresponding_cameras_alignment(
cameras, cameras_tgt, estimate_scale=estimate_scale, mode=mode
)
if batch_size <= 2 and mode == "centers":
# underdetermined case - check only the center alignment error
# since the rotation and translation are ambiguous here
self.assertClose(
cameras_aligned.get_camera_center(),
cameras_tgt.get_camera_center(),
atol=max(add_noise * 7.0, 1e-4),
)
else:
def _rmse(a):
return (torch.norm(a, dim=1, p=2) ** 2).mean().sqrt()
if add_noise != 0.0:
# in a noisy case check mean rotation/translation error for
# extrinsic alignment and root mean center error for center alignment
if mode == "centers":
self.assertNormsClose(
cameras_aligned.get_camera_center(),
cameras_tgt.get_camera_center(),
_rmse,
atol=max(add_noise * 10.0, 1e-4),
)
elif mode == "extrinsics":
angle_err = so3_relative_angle(
cameras_aligned.R, cameras_tgt.R
).mean()
self.assertClose(
angle_err, torch.zeros_like(angle_err), atol=add_noise * 10.0
)
self.assertNormsClose(
cameras_aligned.T, cameras_tgt.T, _rmse, atol=add_noise * 7.0
)
else:
raise ValueError(mode)
else:
# compare the rotations and translations of cameras
self.assertClose(cameras_aligned.R, cameras_tgt.R, atol=3e-4)
self.assertClose(cameras_aligned.T, cameras_tgt.T, atol=3e-4)
# compare the centers
self.assertClose(
cameras_aligned.get_camera_center(),
cameras_tgt.get_camera_center(),
atol=3e-4,
)
def eval_pose(pred, gt):
# ScanNet convention for x_cam ... x_world
# cam->world: right multiply by extrinsics[:3,:3].T and then add extrinsics[:3,3]
# i.e.: x_world = x_cam @ pose[:3, :3].t() + pose[:3, 3]
# i.e.: x_cam = (x_world - pose[:3, 3]) @ pose[:3, :3]
from pytorch3d import ops as pt3ops
from pytorch3d.transforms import so3
ok_gt = torch.isfinite(gt.mean((1, 2))) # some GT poses are NaN
if not ok_gt.any():
return 'NO_GT'
orig_C_pred = pred[ok_gt, :3, 3].clone()
orig_R_pred = pred[ok_gt, :3, :3].clone()
orig_C_gt = gt[ok_gt, :3, 3].clone()
orig_R_gt = gt[ok_gt, :3, :3].clone()
n_frames = orig_C_pred.shape[0]
result = {}
for interpolate in (True, False):
registered = torch.isfinite(orig_C_pred.mean(1))
if not registered.any():
return None
if interpolate: # interpolate NaN cameras
C_pred, R_pred = interpolate_cameras(orig_C_pred, orig_R_pred)
R_gt = orig_R_gt.clone()
C_gt = orig_C_gt.clone()
else: # remove NaN cameras
C_pred = orig_C_pred.clone()[registered > 0]
R_pred = orig_R_pred.clone()[registered > 0]
C_gt = orig_C_gt.clone()[registered > 0]
R_gt = orig_R_gt.clone()[registered > 0]
for align_cams in (True, False):
for estimate_scale in ((True, False) if align_cams else (False,)):
if align_cams:
# estimate the rigid alignment
align_result = pt3ops.corresponding_points_alignment(
C_pred[None], C_gt[None], estimate_scale=estimate_scale)
# align centers and rotations
C_pred_align = (
align_result.s *
C_pred @ align_result.R[0] +
align_result.T[0])
R_pred_align = torch.bmm(
align_result.R.permute(
0, 2, 1).expand_as(R_pred), R_pred)
else:
C_pred_align = C_pred.clone()
R_pred_align = R_pred.clone()
# compute the rotation errors and camera center errors
cam_center_error = (C_pred_align - C_gt).norm(dim=1).mean()
cam_angle_error = so3.so3_relative_angle(
R_pred_align, R_gt).median() * 180 / np.pi
# store the errors
postfix = ''
if not align_cams:
postfix += '_noalign'
if interpolate:
postfix += '_interp'
if estimate_scale:
postfix += '_scale'
result['cam_center_err' + postfix] = float(cam_center_error)
result['cam_angle_err' + postfix] = float(cam_angle_error)
if estimate_scale and not interpolate and align_cams:
result['best_scale'] = float(align_result.s)
return result
