def _extrinsics_rt(self, toref, rt=None):
r'''Get or set the extrinsics in this model
This function represents the pose as a 6-long numpy array that contains
a 3-long Rodrigues rotation followed by a 3-long translation in the last
row:
r = rt[:3]
t = rt[3:]
R = mrcal.R_from_r(r)
The transformation is b <-- R*a + t:
import numpysane as nps
b = nps.matmult(a, nps.transpose(R)) + t
if rt is None: this is a getter; otherwise a setter.
toref is a boolean. if toref: then rt maps points in the coord system of
THIS camera to the reference coord system. Otherwise in the opposite
direction
'''
# The internal representation is rt_fromref
if rt is None:
# getter
if not toref:
return copy.deepcopy(self._extrinsics)
return mrcal.invert_rt(self._extrinsics)
# setter
if not toref:
self._extrinsics = copy.deepcopy(rt)
return True
self._extrinsics = mrcal.invert_rt(rt)
return True
confirm_equal(Rt, invert_Rt(Rt0_ref), msg='invert_Rt result') # in-place Rt0_ref_copy = np.array(Rt0_ref) Rt = mrcal.invert_Rt(Rt0_ref_copy, out=Rt0_ref_copy) confirm_equal(Rt, invert_Rt(Rt0_ref), msg='invert_Rt result written in-place') R = mrcal.invert_R(R0_ref, out=out33) confirm_equal(R, invert_R(R0_ref), msg='invert_R result') # in-place R0_ref_copy = np.array(R0_ref) R = mrcal.invert_R(R0_ref_copy, out=R0_ref_copy) confirm_equal(R, invert_R(R0_ref), msg='invert_R result written in-place') rt = mrcal.invert_rt(rt0_ref, out=out6) confirm_equal(rt, invert_rt(rt0_ref), msg='invert_rt result') # in-place rt0_ref_copy = np.array(rt0_ref) rt = mrcal.invert_rt(rt0_ref_copy, out=rt0_ref_copy) confirm_equal(rt, invert_rt(rt0_ref), msg='invert_rt result written in-place') rt, drt_drt = mrcal.invert_rt(rt0_ref, get_gradients=True, out=(out6, out66)) drt_drt_ref = grad(invert_rt, rt0_ref) confirm_equal(rt, invert_rt(rt0_ref), msg='invert_rt with grad result') confirm_equal(drt_drt, drt_drt_ref, msg='invert_rt drt/drt result') # in-place rt0_ref_copy = np.array(rt0_ref) drt_drt_copy = np.array(drt_drt)
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)
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__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
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)
p)
R_fit = Rt_fit[:3, :]
t_fit = Rt_fit[3, :]
testutils.confirm_equal(R_fit, R, eps=1e-2, msg='Procrustes fit R')
testutils.confirm_equal(t_fit, t, eps=1e-2, msg='Procrustes fit t')
R_fit_vectors = \
mrcal.align_procrustes_vectors_R01(nps.matmult( p, nps.transpose(R) ) + noise,
p)
testutils.confirm_equal(R_fit_vectors,
R,
eps=1e-2,
msg='Procrustes fit R (vectors)')
testutils.confirm_equal(mrcal.invert_Rt(
mrcal.Rt_from_rt(mrcal.invert_rt(mrcal.rt_from_Rt(Rt)))),
Rt,
msg='Rt/rt and invert')
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),
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False,
allow_nonidentity_cam0_transform=False,
range_to_boards=4.0):
r'''Compute a calibration baseline as a starting point for experiments
This is a perfect, noiseless solve. Regularization IS enabled, and the returned
model is at the optimization optimum. So the returned models will not sit
exactly at the ground-truth.
NOTE: if not fixedframes: the ref frame in the returned
optimization_inputs_baseline is NOT the ref frame used by the returned
extrinsics and frames arrays. The arrays in optimization_inputs_baseline had to
be transformed to reference off camera 0. If the extrinsics of camera 0 are the
identity, then the two ref coord systems are the same. To avoid accidental bugs,
we have a kwarg allow_nonidentity_cam0_transform, which defaults to False. if
not allow_nonidentity_cam0_transform and norm(extrinsics_rt_fromref_true[0]) >
0: raise
This logic is here purely for safety. A caller that handles non-identity cam0
transforms has to explicitly say that
ARGUMENTS
- model: string. 'opencv4' or 'opencv8' or 'splined'
- ...
'''
if re.match('opencv', model):
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.opencv8.cameramodel"))
if model == 'opencv4':
# I have opencv8 models_true, but I truncate to opencv4 models_true
for m in models_true:
m.intrinsics(intrinsics=('LENSMODEL_OPENCV4',
m.intrinsics()[1][:8]))
elif model == 'splined':
models_true = (
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam0.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"),
mrcal.cameramodel(f"{testdir}/data/cam1.splined.cameramodel"))
else:
raise Exception("Unknown lens being tested")
models_true = models_true[:Ncameras]
lensmodel = models_true[0].intrinsics()[0]
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
for i in range(Ncameras):
models_true[i].extrinsics_rt_fromref(extrinsics_rt_fromref_true[i])
if not allow_nonidentity_cam0_transform and \
nps.norm2(extrinsics_rt_fromref_true[0]) > 0:
raise Exception(
"A non-identity cam0 transform was given, but the caller didn't explicitly say that they support this"
)
imagersizes = nps.cat(*[m.imagersize() for m in models_true])
# These are perfect
intrinsics_true = nps.cat(*[m.intrinsics()[1] for m in models_true])
extrinsics_true_mounted = nps.cat(
*[m.extrinsics_rt_fromref() for m in models_true])
x_center = -(Ncameras - 1) / 2.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true,Rt_ref_board_true = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, range_to_boards)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, range_to_boards/2.0)),
Nframes)
if extra_observation_at is not None:
q_true_extra,Rt_ref_board_true_extra = \
mrcal.synthesize_board_observations(models_true,
object_width_n, object_height_n, object_spacing,
calobject_warp_true,
np.array((0., 0., 0., x_center, 0, extra_observation_at)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, extra_observation_at/10.0)),
Nframes = 1)
q_true = nps.glue(q_true, q_true_extra, axis=-5)
Rt_ref_board_true = nps.glue(Rt_ref_board_true,
Rt_ref_board_true_extra,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_ref_board_true)
############# I have perfect observations in q_true.
# weight has shape (Nframes, Ncameras, Nh, Nw),
weight01 = (np.random.rand(*q_true.shape[:-1]) + 1.) / 2. # in [0,1]
weight0 = 0.2
weight1 = 1.0
weight = weight0 + (weight1 - weight0) * weight01
if cull_left_of_center:
imagersize = models_true[0].imagersize()
for m in models_true[1:]:
if np.any(m.imagersize() - imagersize):
raise Exception(
"I'm assuming all cameras have the same imager size, but this is false"
)
weight[q_true[..., 0] < imagersize[0] / 2.] /= 1000.
# I want observations of shape (Nframes*Ncameras, Nh, Nw, 3) where each row is
# (x,y,weight)
observations_true = nps.clump(nps.glue(q_true,
nps.dummy(weight, -1),
axis=-1),
n=2)
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros((Nframes * Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:, 0].reshape(Nframes, Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes, dtype=np.int32),
np.ones((Ncameras, ), dtype=np.int32)),
dtype=np.int32)
indices_camera = indices_frame_camera[:, 1].reshape(Nframes, Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes, ), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype=np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
if not fixedframes:
indices_frame_camintrinsics_camextrinsics[:, 2] -= 1
###########################################################################
# p = mrcal.show_geometry(models_true,
# frames = frames_true,
# object_width_n = object_width_n,
# object_height_n = object_height_n,
# object_spacing = object_spacing)
# sys.exit()
# I now reoptimize the perfect-observations problem. Without regularization,
# this is a no-op: I'm already at the optimum. With regularization, this will
# move us a certain amount (that the test will evaluate). Then I look at
# noise-induced motions off this optimization optimum
optimization_inputs_baseline = \
dict( intrinsics = copy.deepcopy(intrinsics_true),
points = None,
observations_board = observations_true,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
lensmodel = lensmodel,
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
do_optimize_frames = not fixedframes,
do_optimize_intrinsics_core = False if model =='splined' else True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = True,
do_optimize_calobject_warp = True,
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if fixedframes:
# Frames are fixed: each camera has an independent pose
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
copy.deepcopy(extrinsics_true_mounted)
optimization_inputs_baseline['frames_rt_toref'] = copy.deepcopy(
frames_true)
else:
# Frames are NOT fixed: cam0 is fixed as the reference coord system. I
# transform each optimization extrinsics vector to be relative to cam0
optimization_inputs_baseline['extrinsics_rt_fromref'] = \
mrcal.compose_rt(extrinsics_true_mounted[1:,:],
mrcal.invert_rt(extrinsics_true_mounted[0,:]))
optimization_inputs_baseline['frames_rt_toref'] = \
mrcal.compose_rt(extrinsics_true_mounted[0,:], frames_true)
mrcal.optimize(**optimization_inputs_baseline)
models_baseline = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs_baseline,
icam_intrinsics = i) \
for i in range(Ncameras) ]
return \
optimization_inputs_baseline, \
models_true, models_baseline, \
indices_frame_camintrinsics_camextrinsics, \
lensmodel, Nintrinsics, imagersizes, \
intrinsics_true, extrinsics_true_mounted, frames_true, \
observations_true, \
Nframes
J_R_ref = grad(r_from_R, Rt0_ref[:3, :]) confirm_equal(rt, rt0_ref, msg='rt_from_Rt result') confirm_equal(J_R, J_R_ref, msg='rt_from_Rt grad result') Rt = mrcal.Rt_from_rt(rt0_ref, out=out43) confirm_equal(Rt, Rt0_ref, msg='Rt_from_rt result') Rt, J_r = mrcal.Rt_from_rt(rt0_ref, get_gradients=True, out=(out43, out333)) J_r_ref = grad(R_from_r, rt0_ref[:3]) confirm_equal(Rt, Rt0_ref, msg='Rt_from_rt result') confirm_equal(J_r, J_r_ref, msg='Rt_from_rt grad result') Rt = mrcal.invert_Rt(Rt0_ref, out=out43) confirm_equal(Rt, invert_Rt(Rt0_ref), msg='invert_Rt result') rt = mrcal.invert_rt(rt0_ref, out=out6) confirm_equal(rt, invert_rt(rt0_ref), msg='invert_rt result') rt, drt_drt = mrcal.invert_rt(rt0_ref, get_gradients=True, out=(out6, out66)) drt_drt_ref = grad(invert_rt, rt0_ref) confirm_equal(rt, invert_rt(rt0_ref), msg='invert_rt with grad result') confirm_equal(drt_drt, drt_drt_ref, msg='invert_rt drt/drt result') Rt2 = mrcal.compose_Rt(Rt0_ref, Rt1_ref, out=out43) confirm_equal(Rt2, compose_Rt(Rt0_ref, Rt1_ref), msg='compose_Rt result') rt2 = mrcal.compose_rt(rt0_ref, rt1_ref, out=out6) confirm_equal(rt2, compose_rt(rt0_ref, rt1_ref), msg='compose_rt result') # _compose_rt() is not excercised by the python library, so I explicitly test it # here
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