out33a = base[3, :3, :3, 2]
out33b = base[3, 3:6, :3, 2]
out33c = base[3, 6:9, :3, 2]
out33d = base[4, :3, :3, 2]
out36 = base[6, :3, :6, 2]
out3 = base[1, 3, 6:9, 1]
out6 = base[1, 4, :6, 1]
out66 = base[5, 3:9, 3:9, 2]
out66a = base[6, 3:9, 3:9, 2]
confirm_equal(mrcal.identity_R(out=out33), np.eye(3), msg='identity_R')
confirm_equal(mrcal.identity_Rt(out=out43),
nps.glue(np.eye(3), np.zeros((3, ), ), axis=-2),
msg='identity_Rt')
confirm_equal(mrcal.identity_r(out=out3), np.zeros((3, )), msg='identity_r')
confirm_equal(mrcal.identity_rt(out=out6), np.zeros((6, )), msg='identity_rt')
################# rotate_point_R
y = \
mrcal.rotate_point_R(R0_ref, x, out = out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)),
msg='rotate_point_R result')
y, J_R, J_x = \
mrcal.rotate_point_R(R0_ref, x, get_gradients=True,
out = (out3,out333,out33))
J_R_ref = grad(lambda R: nps.matmult(x, nps.transpose(R)), R0_ref)
J_x_ref = R0_ref
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)),
testutils.confirm_equal(mrcal.compose_Rt(Rt, mrcal.invert_Rt(Rt)),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='compose_Rt')
testutils.confirm_equal(mrcal.compose_rt(mrcal.rt_from_Rt(Rt),
mrcal.invert_rt(
mrcal.rt_from_Rt(Rt))),
np.zeros((6, )),
msg='compose_rt')
testutils.confirm_equal(mrcal.identity_Rt(),
nps.glue(np.eye(3), np.zeros((3, )), axis=-2),
msg='identity_Rt')
testutils.confirm_equal(mrcal.identity_rt(),
np.zeros((6, )),
msg='identity_rt')
testutils.confirm_equal(mrcal.transform_point_Rt(Rt, p),
Tp,
msg='transform_point_Rt')
testutils.confirm_equal(mrcal.transform_point_rt(mrcal.rt_from_Rt(Rt), p),
Tp,
msg='transform_point_rt')
testutils.confirm_equal(mrcal.transform_point_Rt(mrcal.invert_Rt(Rt), Tp),
p,
msg='transform_point_Rt inverse')