confirm_equal(y, nps.matmult(x, R0_ref), msg='rotate_point_R(inverted) result')
confirm_equal(J_R, J_R_ref, msg='rotate_point_R(inverted) J_R')
confirm_equal(J_x, J_x_ref, msg='rotate_point_R(inverted) J_x')
# inverted in-place
R0_ref_copy = np.array(R0_ref)
x_copy = np.array(x)
y = \
mrcal.rotate_point_R(R0_ref_copy, x_copy, out = x_copy, inverted = True)
confirm_equal(y,
nps.matmult(x, R0_ref),
msg='rotate_point_R result written in-place into x')
################# rotate_point_r
y = mrcal.rotate_point_r(r0_ref, x, out=out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R_from_r(r0_ref))),
msg='rotate_point_r result')
y, J_r, J_x = mrcal.rotate_point_r(r0_ref,
x,
get_gradients=True,
out=(out3, out33, out33a))
J_r_ref = grad(lambda r: nps.matmult(x, nps.transpose(R_from_r(r))), r0_ref)
J_x_ref = grad(lambda x: nps.matmult(x, nps.transpose(R_from_r(r0_ref))), x)
confirm_equal(y,
nps.matmult(x, nps.transpose(R_from_r(r0_ref))),
msg='rotate_point_r result')
confirm_equal(J_r, J_r_ref, msg='rotate_point_r J_r')
confirm_equal(J_x, J_x_ref, msg='rotate_point_r J_x')
el0_deg = 10.0
models_rectified = \
mrcal.rectified_system( (model0, model1),
az_fov_deg = az_fov_deg,
el_fov_deg = el_fov_deg,
el0_deg = el0_deg,
pixels_per_deg_az = -1./8.,
pixels_per_deg_el = -1./4.,
rectification_model = lensmodel)
el0 = el0_deg*np.pi/180.
# az0 is the "forward" direction of the two cameras, relative to the
# baseline vector
baseline = rt01[3:] / nps.mag(rt01[3:])
# "forward" for each of the two cameras, in the cam0 coord system
forward0 = np.array((0,0,1.))
forward1 = mrcal.rotate_point_r(rt01[:3], np.array((0,0,1.)))
forward01 = forward0 + forward1
az0 = np.arcsin( nps.inner(forward01,baseline) / nps.mag(forward01) )
try:
mrcal.stereo._validate_models_rectified(models_rectified)
testutils.confirm(True,
msg=f'Generated models pass validation ({lensmodel})')
except:
testutils.confirm(False,
msg=f'Generated models pass validation ({lensmodel})')
Rt_cam0_rect = mrcal.compose_Rt( model0.extrinsics_Rt_fromref(),
models_rectified[0].extrinsics_Rt_toref())
Rt01_rectified = mrcal.compose_Rt( models_rectified[0].extrinsics_Rt_fromref(),
models_rectified[1].extrinsics_Rt_toref())
def triangulate_nograd(intrinsics_data0,
intrinsics_data1,
rt_cam0_ref,
rt_cam0_ref_baseline,
rt_cam1_ref,
rt_ref_frame,
rt_ref_frame_baseline,
q,
lensmodel,
stabilize_coords=True):
q = nps.atleast_dims(q, -3)
rt01 = mrcal.compose_rt(rt_cam0_ref, mrcal.invert_rt(rt_cam1_ref))
# all the v have shape (...,3)
vlocal0 = \
mrcal.unproject(q[...,0,:],
lensmodel, intrinsics_data0)
vlocal1 = \
mrcal.unproject(q[...,1,:],
lensmodel, intrinsics_data1)
v0 = vlocal0
v1 = \
mrcal.rotate_point_r(rt01[:3], vlocal1)
# The triangulated point in the perturbed camera-0 coordinate system.
# Calibration-time perturbations move this coordinate system, so to get
# a better estimate of the triangulation uncertainty, we try to
# transform this to the original camera-0 coordinate system; the
# stabilization path below does that.
#
# shape (..., 3)
p_triangulated0 = \
mrcal.triangulate_leecivera_mid2(v0, v1, rt01[3:])
if not stabilize_coords:
return p_triangulated0
# Stabilization path. This uses the "true" solution, so I cannot do
# this in the field. But I CAN do this in the randomized trials in
# the test. And I can use the gradients to propagate the uncertainty
# of this computation in the field
#
# Data flow:
# point_cam_perturbed -> point_ref_perturbed -> point_frames
# point_frames -> point_ref_baseline -> point_cam_baseline
p_cam0_perturbed = p_triangulated0
p_ref_perturbed = mrcal.transform_point_rt(rt_cam0_ref,
p_cam0_perturbed,
inverted=True)
# shape (..., Nframes, 3)
p_frames = \
mrcal.transform_point_rt(rt_ref_frame,
nps.dummy(p_ref_perturbed,-2),
inverted = True)
# shape (..., Nframes, 3)
p_ref_baseline_all = mrcal.transform_point_rt(rt_ref_frame_baseline,
p_frames)
# shape (..., 3)
p_ref_baseline = np.mean(p_ref_baseline_all, axis=-2)
# shape (..., 3)
return mrcal.transform_point_rt(rt_cam0_ref_baseline, p_ref_baseline)
def _triangulate(# shape (Ncameras, Nintrinsics)
intrinsics_data,
# shape (Ncameras, 6)
rt_cam_ref,
# shape (Nframes,6),
rt_ref_frame, rt_ref_frame_true,
# shape (..., Ncameras, 2)
q,
lensmodel,
stabilize_coords,
get_gradients):
if not ( intrinsics_data.ndim == 2 and intrinsics_data.shape[0] == 2 and \
rt_cam_ref.shape == (2,6) and \
rt_ref_frame.ndim == 2 and rt_ref_frame.shape[-1] == 6 and \
q.shape[-2:] == (2,2 ) ):
raise Exception("Arguments must have a consistent Ncameras == 2")
# I now compute the same triangulation, but just at the un-perturbed baseline,
# and keeping track of all the gradients
rt0r = rt_cam_ref[0]
rt1r = rt_cam_ref[1]
if not get_gradients:
rtr1 = mrcal.invert_rt(rt1r)
rt01_baseline = mrcal.compose_rt(rt0r, rtr1)
# all the v have shape (...,3)
vlocal0 = \
mrcal.unproject(q[...,0,:],
lensmodel, intrinsics_data[0])
vlocal1 = \
mrcal.unproject(q[...,1,:],
lensmodel, intrinsics_data[1])
v0 = vlocal0
v1 = \
mrcal.rotate_point_r(rt01_baseline[:3], vlocal1)
# p_triangulated has shape (..., 3)
p_triangulated = \
mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:])
if stabilize_coords:
# shape (..., Nframes, 3)
p_frames_new = \
mrcal.transform_point_rt(mrcal.invert_rt(rt_ref_frame),
nps.dummy(p_triangulated,-2))
# shape (..., Nframes, 3)
p_refs = mrcal.transform_point_rt(rt_ref_frame_true, p_frames_new)
# shape (..., 3)
p_triangulated = np.mean(p_refs, axis=-2)
return p_triangulated
else:
rtr1,drtr1_drt1r = mrcal.invert_rt(rt1r,
get_gradients=True)
rt01_baseline,drt01_drt0r, drt01_drtr1 = mrcal.compose_rt(rt0r, rtr1, get_gradients=True)
# all the v have shape (...,3)
vlocal0, dvlocal0_dq0, dvlocal0_dintrinsics0 = \
mrcal.unproject(q[...,0,:],
lensmodel, intrinsics_data[0],
get_gradients = True)
vlocal1, dvlocal1_dq1, dvlocal1_dintrinsics1 = \
mrcal.unproject(q[...,1,:],
lensmodel, intrinsics_data[1],
get_gradients = True)
v0 = vlocal0
v1, dv1_dr01, dv1_dvlocal1 = \
mrcal.rotate_point_r(rt01_baseline[:3], vlocal1,
get_gradients=True)
# p_triangulated has shape (..., 3)
p_triangulated, dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01 = \
mrcal.triangulate_leecivera_mid2(v0, v1, rt01_baseline[3:],
get_gradients = True)
shape_leading = dp_triangulated_dv0.shape[:-2]
dp_triangulated_dq = np.zeros(shape_leading + (3,) + q.shape[-2:], dtype=float)
nps.matmult( dp_triangulated_dv0,
dvlocal0_dq0,
out = dp_triangulated_dq[..., 0, :])
nps.matmult( dp_triangulated_dv1,
dv1_dvlocal1,
dvlocal1_dq1,
out = dp_triangulated_dq[..., 1, :])
Nframes = len(rt_ref_frame)
if stabilize_coords:
# shape (Nframes,6)
rt_frame_ref, drtfr_drtrf = \
mrcal.invert_rt(rt_ref_frame, get_gradients=True)
# shape (Nframes,6)
rt_true_shifted, _, drt_drtfr = \
mrcal.compose_rt(rt_ref_frame_true, rt_frame_ref,
get_gradients=True)
# shape (..., Nframes, 3)
p_refs,dprefs_drt,dprefs_dptriangulated = \
mrcal.transform_point_rt(rt_true_shifted,
nps.dummy(p_triangulated,-2),
get_gradients = True)
# shape (..., 3)
p_triangulated = np.mean(p_refs, axis=-2)
# I have dpold/dx. dpnew/dx = dpnew/dpold dpold/dx
# shape (...,3,3)
dpnew_dpold = np.mean(dprefs_dptriangulated, axis=-3)
dp_triangulated_dv0 = nps.matmult(dpnew_dpold, dp_triangulated_dv0)
dp_triangulated_dv1 = nps.matmult(dpnew_dpold, dp_triangulated_dv1)
dp_triangulated_dt01 = nps.matmult(dpnew_dpold, dp_triangulated_dt01)
dp_triangulated_dq = nps.xchg(nps.matmult( dpnew_dpold,
nps.xchg(dp_triangulated_dq,
-2,-3)),
-2,-3)
# shape (..., Nframes,3,6)
dp_triangulated_drtrf = \
nps.matmult(dprefs_drt, drt_drtfr, drtfr_drtrf) / Nframes
else:
dp_triangulated_drtrf = np.zeros(shape_leading + (Nframes,3,6), dtype=float)
return \
p_triangulated, \
drtr1_drt1r, \
drt01_drt0r, drt01_drtr1, \
dvlocal0_dintrinsics0, dvlocal1_dintrinsics1, \
dv1_dr01, dv1_dvlocal1, \
dp_triangulated_dv0, dp_triangulated_dv1, dp_triangulated_dt01, \
dp_triangulated_drtrf, \
dp_triangulated_dq