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)
# in-place
Rt0_ref_copy = np.array(Rt0_ref)
Rt1_ref_copy = np.array(Rt1_ref)
Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy, out=Rt0_ref_copy)
confirm_equal(Rt2,
compose_Rt(Rt0_ref, Rt1_ref),
msg='compose_Rt result written in-place to Rt0')
Rt0_ref_copy = np.array(Rt0_ref)
Rt1_ref_copy = np.array(Rt1_ref)
Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy, out=Rt1_ref_copy)
confirm_equal(Rt2,
compose_Rt(Rt0_ref, Rt1_ref),
msg='compose_Rt result written in-place to Rt1')
rt2 = mrcal.compose_rt(rt0_ref, rt1_ref, out=out6)
confirm_equal(rt2, compose_rt(rt0_ref, rt1_ref), msg='compose_rt result')
# in-place
rt0_ref_copy = np.array(rt0_ref)
rt1_ref_copy = np.array(rt1_ref)
rt2 = mrcal.compose_rt(rt0_ref_copy, rt1_ref_copy, out=rt0_ref_copy)
confirm_equal(rt2,
compose_rt(rt0_ref, rt1_ref),
msg='compose_rt result written in-place to rt0')
rt0_ref_copy = np.array(rt0_ref)
rt1_ref_copy = np.array(rt1_ref)
rt2 = mrcal.compose_rt(rt0_ref_copy, rt1_ref_copy, out=rt1_ref_copy)
confirm_equal(rt2,
compose_rt(rt0_ref, rt1_ref),
msg='compose_rt result written in-place to rt1')
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/..",
import mrcal
import scipy.interpolate
import testutils
model0 = mrcal.cameramodel(f"{testdir}/data/cam0.opencv8.cameramodel")
model1 = mrcal.cameramodel(model0)
for lensmodel in ('LENSMODEL_LATLON', 'LENSMODEL_PINHOLE'):
# I create geometries to test. First off, a vanilla geometry for left-right stereo
rt01 = np.array((0,0,0, 3.0, 0, 0))
model1.extrinsics_rt_toref( mrcal.compose_rt(model0.extrinsics_rt_toref(),
rt01))
az_fov_deg = 90
el_fov_deg = 50
models_rectified = \
mrcal.rectified_system( (model0, model1),
az_fov_deg = az_fov_deg,
el_fov_deg = el_fov_deg,
pixels_per_deg_az = -1./8.,
pixels_per_deg_el = -1./4.,
rectification_model = lensmodel)
az0 = 0.
el0 = 0.
try:
mrcal.stereo._validate_models_rectified(models_rectified)
observations = observations[i, ...]
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics[
i, ...]
paths = [paths[_] for _ in i]
# reference models
models = [
mrcal.cameramodel(m) for m in (
f"{testdir}/data/cam0.opencv8.cameramodel",
f"{testdir}/data/cam1.opencv8.cameramodel",
)
]
lensmodel = models[0].intrinsics()[0]
intrinsics_data = nps.cat(models[0].intrinsics()[1], models[1].intrinsics()[1])
extrinsics_rt_fromref = mrcal.compose_rt(models[1].extrinsics_rt_fromref(),
models[0].extrinsics_rt_toref())
imagersizes = nps.cat(models[0].imagersize(), models[1].imagersize())
# I now have the "right" camera parameters. I don't have the frames or points,
# but it's fine to just make them up. This is a regression test.
frames_rt_toref = linspace_shaped(3, 6)
frames_rt_toref[:, 5] += 5 # push them back
indices_point_camintrinsics_camextrinsics = \
np.array(((0,1,-1),
(1,0,-1),
(1,1, 0),
(2,0,-1),
(2,1, 0)),
dtype = np.int32)
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),
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')
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
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
rt2 = _poseutils._compose_rt(rt0_ref, rt1_ref, out=out6)
confirm_equal(rt2,
compose_rt(rt0_ref, rt1_ref),
msg='compose_rt result; calling _compose_rt() directly')
rt2,drt2_drt0,drt2_drt1 = \
mrcal.compose_rt(rt0_ref, rt1_ref, get_gradients=True,
out = (out6, out66, out66a))
drt2_drt0_ref = grad(lambda rt0: compose_rt(rt0, rt1_ref), rt0_ref)
drt2_drt1_ref = grad(lambda rt1: compose_rt(rt0_ref, rt1), rt1_ref)
print(f"Usage: {sys.argv[0]} model image yaw_deg what", file=sys.stderr)
sys.exit(1)
# I want a pinhole model to cover the middle 1/3rd of my pixels
W, H = model.imagersize()
fit_points = \
np.array((( W/3., H/3.),
( W*2./3., H/3.),
( W/3., H*2./3.),
( W*2./3., H*2./3.)))
model_pinhole = \
mrcal.pinhole_model_for_reprojection(model,
fit = fit_points,
scale_image = 0.5)
# yaw transformation: pure rotation around the y axis
rt_yaw = np.array((0., yaw_deg * np.pi / 180., 0, 0, 0, 0))
# apply the extra yaw transformation to my extrinsics
model_pinhole.extrinsics_rt_toref( \
mrcal.compose_rt(model_pinhole.extrinsics_rt_toref(),
rt_yaw) )
mapxy = mrcal.image_transformation_map(model, model_pinhole, use_rotation=True)
image_transformed = mrcal.transform_image(image, mapxy)
cv2.imwrite(f'/tmp/narrow-{what}.jpg', image_transformed)
model_pinhole.write(f'/tmp/pinhole-narrow-yawed-{what}.cameramodel')
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