def _validateIntrinsics(imagersize,
i,
optimization_inputs = None,
icam_intrinsics = None):
r'''Raises an exception if given components of the intrinsics is invalid'''
# need two integers in the imager size
try:
N = len(imagersize)
if N != 2:
raise Exception("The imagersize must be an iterable of two positive integers")
if imagersize[0] <= 0 or imagersize[1] <= 0:
raise Exception("The imagersize must be an iterable of two positive integers")
if imagersize[0] != int(imagersize[0]) or imagersize[1] != int(imagersize[1]):
raise Exception("The imagersize must be an iterable of two positive integers")
except:
raise Exception("The imagersize must be an iterable of two positive integers")
try:
N = len(i)
except:
raise Exception("Valid intrinsics are an iterable of len 2")
if N != 2:
raise Exception("Valid intrinsics are an iterable of len 2")
lensmodel = i[0]
intrinsics = i[1]
# If this fails, I keep the exception and let it fall through
Nintrinsics_want = mrcal.lensmodel_num_params(lensmodel)
try:
Nintrinsics_have = len(intrinsics)
except:
raise Exception("Valid intrinsics are (lensmodel, intrinsics) where 'intrinsics' is an iterable with a length")
if Nintrinsics_want != Nintrinsics_have:
raise Exception("Mismatched Nintrinsics. Got {}, but model {} must have {}".format(Nintrinsics_have,lensmodel,Nintrinsics_want))
for x in intrinsics:
if not isinstance(x, numbers.Number):
raise Exception("All intrinsics elements should be numeric, but '{}' isn't".format(x))
if optimization_inputs is not None:
# Currently this is only checked when we set the optimization_inputs by
# calling intrinsics(). We do NOT check this if reading a file from
# disk. This is done as an optimization: we store the unprocessed
# _optimization_inputs_string bytes and only decode them as needed.
# Perhaps I should expand this
if not isinstance(optimization_inputs, dict):
raise Exception(f"'optimization_inputs' must be a dict. Instead got type {type(optimization_inputs)}")
if icam_intrinsics is None:
raise Exception(f"optimization_inputs is given, so icam_intrinsics MUST be given too")
if not isinstance(icam_intrinsics, int):
raise Exception(f"icam_intrinsics not an int. This must be an int >= 0")
if icam_intrinsics < 0:
raise Exception(f"icam_intrinsics < 0. This must be an int >= 0")
def ingest_packed_state(p_packed, **optimization_inputs):
r'''Read a given packed state into optimization_inputs
SYNOPSIS
# A simple gradient check
model = mrcal.cameramodel('xxx.cameramodel')
optimization_inputs = model.optimization_inputs()
p0,x0,J = mrcal.optimizer_callback(no_factorization = True,
**optimization_inputs)[:3]
dp = np.random.randn(len(p0)) * 1e-9
mrcal.ingest_packed_state(p0 + dp,
**optimization_inputs)
x1 = mrcal.optimizer_callback(no_factorization = True,
no_jacobian = True,
**optimization_inputs)[1]
dx_observed = x1 - x0
dx_predicted = nps.inner(J, dp_packed)
This is the converse of mrcal.optimizer_callback(). One thing
mrcal.optimizer_callback() does is to convert the expanded (intrinsics,
extrinsics, ...) arrays into a 1-dimensional scaled optimization vector
p_packed. mrcal.ingest_packed_state() allows updates to p_packed to be absorbed
back into the (intrinsics, extrinsics, ...) arrays for further evaluation with
mrcal.optimizer_callback() and others.
ARGUMENTS
- p_packed: a numpy array of shape (Nstate,) containing the input packed state
- **optimization_inputs: a dict() of arguments passable to mrcal.optimize() and
mrcal.optimizer_callback(). The arrays in this dict are updated
RETURNED VALUE
None
'''
intrinsics = optimization_inputs.get("intrinsics")
extrinsics = optimization_inputs.get("extrinsics_rt_fromref")
frames = optimization_inputs.get("frames_rt_toref")
points = optimization_inputs.get("points")
calobject_warp = optimization_inputs.get("calobject_warp")
Npoints_fixed = optimization_inputs.get('Npoints_fixed', 0)
Nvars_intrinsics = mrcal.num_states_intrinsics(**optimization_inputs)
Nvars_extrinsics = mrcal.num_states_extrinsics(**optimization_inputs)
Nvars_frames = mrcal.num_states_frames(**optimization_inputs)
Nvars_points = mrcal.num_states_points(**optimization_inputs)
Nvars_calobject_warp = mrcal.num_states_calobject_warp(
**optimization_inputs)
Nvars_expected = \
Nvars_intrinsics + \
Nvars_extrinsics + \
Nvars_frames + \
Nvars_points + \
Nvars_calobject_warp
# Defaults MUST match those in OPTIMIZER_ARGUMENTS_OPTIONAL in
# mrcal-pywrap.c. Or better yet, this whole function should
# come from the C code instead of being reimplemented here in Python
do_optimize_intrinsics_core = optimization_inputs.get(
'do_optimize_intrinsics_core', True)
do_optimize_intrinsics_distortions = optimization_inputs.get(
'do_optimize_intrinsics_distortions', True)
do_optimize_extrinsics = optimization_inputs.get('do_optimize_extrinsics',
True)
do_optimize_frames = optimization_inputs.get('do_optimize_frames', True)
do_optimize_calobject_warp = optimization_inputs.get(
'do_optimize_calobject_warp', True)
if p_packed.ravel().size != Nvars_expected:
raise Exception(
f"Mismatched array size: p_packed.size={p_packed.ravel().size} while the optimization problem expects {Nvars_expected}"
)
p = p_packed.copy()
mrcal.unpack_state(p, **optimization_inputs)
if do_optimize_intrinsics_core or \
do_optimize_intrinsics_distortions:
ivar0 = mrcal.state_index_intrinsics(0, **optimization_inputs)
if ivar0 is not None:
iunpacked0, iunpacked1 = None, None # everything by default
lensmodel = optimization_inputs['lensmodel']
has_core = mrcal.lensmodel_metadata_and_config(
lensmodel)['has_core']
Ncore = 4 if has_core else 0
Ndistortions = mrcal.lensmodel_num_params(lensmodel) - Ncore
if not do_optimize_intrinsics_core:
iunpacked0 = Ncore
if not do_optimize_intrinsics_distortions:
iunpacked1 = -Ndistortions
intrinsics[:, iunpacked0:iunpacked1].ravel()[:] = \
p[ ivar0:Nvars_intrinsics ]
if do_optimize_extrinsics:
ivar0 = mrcal.state_index_extrinsics(0, **optimization_inputs)
if ivar0 is not None:
extrinsics.ravel()[:] = p[ivar0:ivar0 + Nvars_extrinsics]
if do_optimize_frames:
ivar0 = mrcal.state_index_frames(0, **optimization_inputs)
if ivar0 is not None:
frames.ravel()[:] = p[ivar0:ivar0 + Nvars_frames]
if do_optimize_frames:
ivar0 = mrcal.state_index_points(0, **optimization_inputs)
if ivar0 is not None:
points.ravel()[:-Npoints_fixed * 3] = p[ivar0:ivar0 + Nvars_points]
if do_optimize_calobject_warp:
ivar0 = mrcal.state_index_calobject_warp(**optimization_inputs)
if ivar0 is not None:
calobject_warp.ravel()[:] = p[ivar0:ivar0 + Nvars_calobject_warp]
############# Set up my world, and compute all the perfect positions, pixel
############# observations of everything
models_ref = (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"))
imagersizes = nps.cat(*[m.imagersize() for m in models_ref])
lensmodel = models_ref[0].intrinsics()[0]
# I have opencv8 models_ref, but let me truncate to opencv4 models_ref to keep this
# simple and fast
lensmodel = 'LENSMODEL_OPENCV4'
for m in models_ref:
m.intrinsics(intrinsics=(lensmodel, m.intrinsics()[1][:8]))
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
Ncameras = len(models_ref)
Ncameras_extrinsics = Ncameras - 1
Nframes = 50
models_ref[0].extrinsics_rt_fromref(np.zeros((6, ), dtype=float))
models_ref[1].extrinsics_rt_fromref(np.array((0.08, 0.2, 0.02, 1., 0.9, 0.1)))
models_ref[2].extrinsics_rt_fromref(np.array((0.01, 0.07, 0.2, 2.1, 0.4, 0.2)))
models_ref[3].extrinsics_rt_fromref(np.array(
(-0.1, 0.08, 0.08, 4.4, 0.2, 0.1)))
pixel_uncertainty_stdev = 1.5
object_spacing = 0.1
object_width_n = 10
def _write(f, m, note=None):
r'''Writes a cameramodel as a .cahvor to a writeable file object'''
if note is not None:
for l in note.splitlines():
f.write('# ' + l + '\n')
d = m.imagersize()
f.write('Dimensions = {} {}\n'.format(int(d[0]), int(d[1])))
lensmodel,intrinsics = m.intrinsics()
if lensmodel == 'LENSMODEL_CAHVOR':
f.write("Model = CAHVOR = perspective, distortion\n")
elif re.match('LENSMODEL_(OPENCV.*|PINHOLE)', lensmodel):
f.write("Model = CAHV = perspective, linear\n")
else:
match = re.match('^LENSMODEL_CAHVORE_linearity=([0-9\.]+)$', lensmodel)
if match is not None:
f.write("Model = CAHVORE3,{} = general\n".format(match.group(1)))
else:
raise Exception("Don't know how to handle lens model '{}'".format(lensmodel))
fx,fy,cx,cy = intrinsics[:4]
Rt_toref = m.extrinsics_Rt_toref()
R_toref = Rt_toref[:3,:]
t_toref = Rt_toref[ 3,:]
C = t_toref
A = R_toref[:,2]
Hp = R_toref[:,0]
Vp = R_toref[:,1]
H = fx*Hp + A*cx
V = fy*Vp + A*cy
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('C', *C))
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('A', *A))
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('H', *H))
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('V', *V))
if re.match('^LENSMODEL_CAHVOR', lensmodel):
# CAHVOR(E)
alpha,beta,R0,R1,R2 = intrinsics[4:9]
s_al,c_al,s_be,c_be = np.sin(alpha),np.cos(alpha),np.sin(beta),np.cos(beta)
O = nps.matmult( R_toref, nps.transpose(np.array(( s_al*c_be, s_be, c_al*c_be ), dtype=float)) ).ravel()
R = np.array((R0, R1, R2), dtype=float)
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('O', *O))
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('R', *R))
if re.match('^LENSMODEL_CAHVORE', lensmodel):
E = intrinsics[9:]
f.write(("{} =" + (" {:15.10f}" * 3) + "\n").format('E', *E))
elif re.match('LENSMODEL_OPENCV*', lensmodel):
Ndistortions = mrcal.lensmodel_num_params(lensmodel) - 4
f.write(("{} =" + (" {:15.10f}" * Ndistortions) + "\n").format(lensmodel, *intrinsics[4:]))
elif lensmodel == 'LENSMODEL_PINHOLE':
# the CAHV values we already wrote are all that's needed
pass
else:
raise Exception(f"Cannot write lens model '{lensmodel}' to a .cahvor file. I only support PINHOLE, CAHVOR(E) and OPENCV model")
c = m.valid_intrinsics_region()
if c is not None:
f.write("VALID_INTRINSICS_REGION = ")
np.savetxt(f, c.ravel(), fmt='%.2f', newline=' ')
f.write('\n')
Hs,Vs,Hc,Vc = intrinsics[:4]
f.write("Hs = {}\n".format(Hs))
f.write("Hc = {}\n".format(Hc))
f.write("Vs = {}\n".format(Vs))
f.write("Vc = {}\n".format(Vc))
f.write("# this is hard-coded\nTheta = {} (-90.0 deg)\n".format(-np.pi/2))
return True
2.112936851,1.126412871,2.113220553,1.114991063,2.017901873,1.244588667,
2.051238803,1.201855728,2.043256406,1.216674722,2.035286046,1.178380907,
2.08028318,1.178783085,2.051214271,1.173560417,2.059298121,1.182414688,
2.094607679,1.177960959,2.086998287,1.147371259,2.12029442,1.138197348,
2.138994213, 1.114846113,])))
# a few points, some wide, some not. None behind the camera
p = np.array(((1.0, 2.0, 10.0), (-1.1, 0.3, 1.0), (-0.9, -1.5, 1.0)))
delta = 1e-6
for i in intrinsics:
q, dq_dp, dq_di = mrcal.project(p, *i, get_gradients=True)
Nintrinsics = mrcal.lensmodel_num_params(i[0])
testutils.confirm_equal(dq_di.shape[-1],
Nintrinsics,
msg=f"{i[0]}: Nintrinsics match for {i[0]}")
if Nintrinsics != dq_di.shape[-1]:
continue
for ivar in range(dq_dp.shape[-1]):
# center differences
p1 = p.copy()
p1[..., ivar] = p[..., ivar] - delta / 2
q1 = mrcal.project(p1, *i, get_gradients=False)
p1[..., ivar] += delta
q2 = mrcal.project(p1, *i, get_gradients=False)
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
def calibration_baseline(model,
Ncameras,
Nframes,
extra_observation_at,
pixel_uncertainty_stdev,
object_width_n,
object_height_n,
object_spacing,
extrinsics_rt_fromref_true,
calobject_warp_true,
fixedframes,
testdir,
cull_left_of_center=False):
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
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])
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_cam0_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, 4.0)),
np.array((np.pi/180.*30., np.pi/180.*30., np.pi/180.*20., 2.5, 2.5, 2.0)),
Nframes)
if extra_observation_at:
c = mrcal.ref_calibration_object(object_width_n, object_height_n,
object_spacing, calobject_warp_true)
Rt_cam0_board_true_far = \
nps.glue( np.eye(3),
np.array((0,0,extra_observation_at)),
axis=-2)
q_true_far = \
mrcal.project(mrcal.transform_point_Rt(Rt_cam0_board_true_far, c),
*models_true[0].intrinsics())
q_true = nps.glue(q_true_far, q_true, axis=-5)
Rt_cam0_board_true = nps.glue(Rt_cam0_board_true_far,
Rt_cam0_board_true,
axis=-3)
Nframes += 1
frames_true = mrcal.rt_from_Rt(Rt_cam0_board_true)
############# I have perfect observations in q_true. I corrupt them by noise
# 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
###########################################################################
# Now I apply pixel noise, and look at the effects on the resulting calibration.
# 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),
extrinsics_rt_fromref = copy.deepcopy(extrinsics_true_mounted if fixedframes else extrinsics_true_mounted[1:,:]),
frames_rt_toref = copy.deepcopy(frames_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,
observed_pixel_uncertainty = pixel_uncertainty_stdev,
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)
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
