confirm_equal(
J_Rt,
J_Rt_ref,
msg=
'transform_point_Rt(inverted) (with gradients) result written in-place into x: J_Rt'
)
confirm_equal(
J_x,
J_x_ref,
msg=
'transform_point_Rt(inverted) (with gradients) result written in-place into x: J_x'
)
################# transform_point_rt
y = mrcal.transform_point_rt(rt0_ref, x, out=out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_rt result')
y, J_rt, J_x = mrcal.transform_point_rt(rt0_ref,
x,
get_gradients=True,
out=(out3, out36, out33a))
J_rt_ref = grad(
lambda rt: nps.matmult(x, nps.transpose(R_from_r(rt[:3]))) + rt[3:],
rt0_ref)
J_x_ref = grad(lambda x: nps.matmult(x, nps.transpose(R0_ref)) + t0_ref, x)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_rt result')
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)
lensmodel, intrinsics_data = m.intrinsics()
ref_p = np.array(
((10., 20., 100.), (25., 30., 90.), (5., 10., 94.), (-45., -20., 95.),
(-35., 14., 77.), (5., -0., 110.), (1., 50., 50.)))
# The points are all somewhere at +z. So the Camera poses are all ~ identity
ref_extrinsics_rt_fromref = np.array(
((-0.1, -0.07, 0.01, 10.0, 4.0, -7.0), (-0.01, 0.05, -0.02, 30.0, -8.0,
-8.0), (-0.1, 0.03, -0.03, 10.0,
-9.0, 20.0),
(0.04, -0.04, 0.03, -20.0, 2.0, -11.0), (0.01, 0.05, -0.05, -10.0, 3.0,
9.0)))
# shape (Ncamposes, Npoints, 3)
ref_p_cam = mrcal.transform_point_rt(nps.mv(ref_extrinsics_rt_fromref, -2, -3),
ref_p)
# shape (Ncamposes, Npoints, 2)
ref_q_cam = mrcal.project(ref_p_cam, lensmodel, intrinsics_data)
# Observations are incomplete. Not all points are observed from everywhere
indices_point_camintrinsics_camextrinsics = \
np.array(((0, 0, 1),
(0, 0, 2),
(0, 0, 4),
(1, 0, 0),
(1, 0, 1),
(1, 0, 4),
(2, 0, 0),
(2, 0, 1),
(2, 0, 2),
def reproject_perturbed__fit_boards_ref(
q,
distance,
# shape (Ncameras, Nintrinsics)
baseline_intrinsics,
# shape (Ncameras, 6)
baseline_rt_cam_ref,
# shape (Nframes, 6)
baseline_rt_ref_frame,
# shape (2)
baseline_calobject_warp,
# shape (..., Ncameras, Nintrinsics)
query_intrinsics,
# shape (..., Ncameras, 6)
query_rt_cam_ref,
# shape (..., Nframes, 6)
query_rt_ref_frame,
# shape (..., 2)
query_calobject_warp):
r'''Reproject by explicitly computing a procrustes fit to align the reference
coordinate systems of the two solves. We match up the two sets of chessboard
points
'''
calobject_height, calobject_width = optimization_inputs_baseline[
'observations_board'].shape[1:3]
# shape (Nsamples, Nh, Nw, 3)
if query_calobject_warp.ndim > 1:
calibration_object_query = \
nps.cat(*[ mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=calobject_warp) \
for calobject_warp in query_calobject_warp] )
else:
calibration_object_query = \
mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=query_calobject_warp)
# shape (Nsamples, Nframes, Nh, Nw, 3)
pcorners_ref_query = \
mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2, -2),
nps.dummy(calibration_object_query, -4))
# shape (Nh, Nw, 3)
calibration_object_baseline = \
mrcal.ref_calibration_object(calobject_width, calobject_height,
optimization_inputs_baseline['calibration_object_spacing'],
calobject_warp=baseline_calobject_warp)
# frames_ref.shape is (Nframes, 6)
# shape (Nframes, Nh, Nw, 3)
pcorners_ref_baseline = \
mrcal.transform_point_rt( nps.dummy(baseline_rt_ref_frame, -2, -2),
calibration_object_baseline)
# shape (Nsamples,4,3)
Rt_refq_refb = \
mrcal.align_procrustes_points_Rt01( \
# shape (Nsamples,N,3)
nps.mv(nps.clump(nps.mv(pcorners_ref_query, -1,0),n=-3),0,-1),
# shape (N,3)
nps.clump(pcorners_ref_baseline, n=3))
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
# shape (Ncameras, 3). In the ref coord system
p_ref_baseline = \
mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref),
p_cam_baseline )
# shape (Nsamples,Ncameras,3)
p_ref_query = \
mrcal.transform_point_Rt(nps.mv(Rt_refq_refb,-3,-4),
p_ref_baseline)
# shape (..., Ncameras, 3)
p_cam_query = \
mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query)
# shape (..., Ncameras, 2)
q1 = mrcal.project(p_cam_query, lensmodel, query_intrinsics)
if q1.shape[-3] == 1: q1 = q1[0, :, :]
return q1
v1[0] = 0
el_fov_deg_observed = np.arccos(nps.inner(v0,v1)/(nps.mag(v0)*nps.mag(v1))) * 180./np.pi
testutils.confirm_equal(el_fov_deg_observed,
el_fov_deg,
msg=f'complex stereo: el_fov ({lensmodel})',
eps = 0.5)
# I examine points somewhere in space. I make sure the rectification maps
# transform it properly. And I compute what its az,el and disparity would have
# been, and I check the geometric functions
pcam0 = np.array((( 1., 2., 12.),
(-4., 3., 12.)))
qcam0 = mrcal.project( pcam0, *model0.intrinsics() )
pcam1 = mrcal.transform_point_rt(mrcal.invert_rt(rt01), pcam0)
qcam1 = mrcal.project( pcam1, *model1.intrinsics() )
prect0 = mrcal.transform_point_Rt( mrcal.invert_Rt(Rt_cam0_rect), pcam0)
prect1 = prect0 - Rt01_rectified[3,:]
qrect0 = mrcal.project(prect0, *models_rectified[0].intrinsics())
qrect1 = mrcal.project(prect1, *models_rectified[1].intrinsics())
Naz,Nel = models_rectified[0].imagersize()
row = np.arange(Naz, dtype=float)
col = np.arange(Nel, dtype=float)
rectification_maps = mrcal.rectification_maps((model0,model1),
models_rectified)
def reproject_perturbed__mean_frames(
q,
distance,
# shape (Ncameras, Nintrinsics)
baseline_intrinsics,
# shape (Ncameras, 6)
baseline_rt_cam_ref,
# shape (Nframes, 6)
baseline_rt_ref_frame,
# shape (2)
baseline_calobject_warp,
# shape (..., Ncameras, Nintrinsics)
query_intrinsics,
# shape (..., Ncameras, 6)
query_rt_cam_ref,
# shape (..., Nframes, 6)
query_rt_ref_frame,
# shape (..., 2)
query_calobject_warp):
r'''Reproject by computing the mean in the space of frames
This is what the uncertainty computation does (as of 2020/10/26). The implied
rotation here is aphysical (it is a mean of multiple rotation matrices)
'''
# shape (Ncameras, 3)
p_cam_baseline = mrcal.unproject(
q, lensmodel, baseline_intrinsics, normalize=True) * distance
# shape (Ncameras, 3)
p_ref_baseline = \
mrcal.transform_point_rt( mrcal.invert_rt(baseline_rt_cam_ref),
p_cam_baseline )
if fixedframes:
p_ref_query = p_ref_baseline
else:
# shape (Nframes, Ncameras, 3)
# The point in the coord system of all the frames
p_frames = mrcal.transform_point_rt( \
nps.dummy(mrcal.invert_rt(baseline_rt_ref_frame),-2),
p_ref_baseline)
# shape (..., Nframes, Ncameras, 3)
p_ref_query_allframes = \
mrcal.transform_point_rt( nps.dummy(query_rt_ref_frame, -2),
p_frames )
if args.reproject_perturbed == 'mean-frames':
# "Normal" path: I take the mean of all the frame-coord-system
# representations of my point
if not fixedframes:
# shape (..., Ncameras, 3)
p_ref_query = np.mean(p_ref_query_allframes, axis=-3)
# shape (..., Ncameras, 3)
p_cam_query = \
mrcal.transform_point_rt(query_rt_cam_ref, p_ref_query)
# shape (..., Ncameras, 2)
return mrcal.project(p_cam_query, lensmodel, query_intrinsics)
else:
# Experimental path: I take the mean of the projections, not the points
# in the reference frame
# guaranteed that not fixedframes: I asserted this above
# shape (..., Nframes, Ncameras, 3)
p_cam_query_allframes = \
mrcal.transform_point_rt(nps.dummy(query_rt_cam_ref, -3),
p_ref_query_allframes)
# shape (..., Nframes, Ncameras, 2)
q_reprojected = mrcal.project(p_cam_query_allframes, lensmodel,
nps.dummy(query_intrinsics, -3))
if args.reproject_perturbed != 'mean-frames-using-meanq-penalize-big-shifts':
return np.mean(q_reprojected, axis=-3)
else:
# Experiment. Weighted mean to de-emphasize points with huge shifts
w = 1. / nps.mag(q_reprojected - q)
w = nps.mv(nps.cat(w, w), 0, -1)
return \
np.sum(q_reprojected*w, axis=-3) / \
np.sum(w, axis=-3)
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')
testutils.confirm_equal(mrcal.transform_point_rt(
mrcal.invert_rt(mrcal.rt_from_Rt(Rt)), Tp),
p,
msg='transform_point_rt inverse')
testutils.confirm_equal(mrcal.R_from_quat(mrcal.quat_from_R(R)),
R,
msg='quaternion stuff')
# modified extrinsics such that the old-intrinsics and new-intrinsics project
# world points to the same place
out = subprocess.check_output( (f"{testdir}/../mrcal-graft-models",
'--radius', '-1',
filename0, filename1),
encoding = 'ascii',
stderr = subprocess.DEVNULL)
filename01_compensated = f"{workdir}/model01_compensated.cameramodel"
with open(filename01_compensated, "w") as f:
print(out, file=f)
model01_compensated = mrcal.cameramodel(filename01_compensated)
p1 = np.array((11., 17., 10000.))
pref = mrcal.transform_point_rt( model1.extrinsics_rt_toref(),
p1)
q = mrcal.project( mrcal.transform_point_rt( model1.extrinsics_rt_fromref(),
pref ),
*model1.intrinsics())
q_compensated = \
mrcal.project( mrcal.transform_point_rt( model01_compensated.extrinsics_rt_fromref(),
pref),
*model01_compensated.intrinsics())
testutils.confirm_equal(q_compensated, q,
msg = f"Compensated projection ended up in the same place",
eps = 0.1)
testutils.finish()
msg='transform_point_Rt result')
y, J_Rt, J_x = mrcal.transform_point_Rt(Rt0_ref,
x,
get_gradients=True,
out=(out3, out343, out33))
J_Rt_ref = grad(lambda Rt: nps.matmult(x, nps.transpose(Rt[:3, :])) + Rt[3, :],
Rt0_ref)
J_x_ref = R0_ref
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_Rt result')
confirm_equal(J_Rt, J_Rt_ref, msg='transform_point_Rt J_Rt')
confirm_equal(J_x, J_x_ref, msg='transform_point_Rt J_x')
y = mrcal.transform_point_rt(rt0_ref, x, out=out3)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_rt result')
y, J_rt, J_x = mrcal.transform_point_rt(rt0_ref,
x,
get_gradients=True,
out=(out3, out36, out33a))
J_rt_ref = grad(
lambda rt: nps.matmult(x, nps.transpose(R_from_r(rt[:3]))) + rt[3:],
rt0_ref)
J_x_ref = grad(lambda x: nps.matmult(x, nps.transpose(R0_ref)) + t0_ref, x)
confirm_equal(y,
nps.matmult(x, nps.transpose(R0_ref)) + t0_ref,
msg='transform_point_rt result')
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
