relative=True,
worstcase=True,
eps=5e-3,
msg="Gradient check: dp_triangulated_dpstate[extrinsics0]")
if istate_e1 is not None:
testutils.confirm_equal(
dp_triangulated_dpstate[..., istate_e1:istate_e1 + 6],
dp_triangulated_de1_empirical,
relative=True,
worstcase=True,
eps=5e-3,
msg="Gradient check: dp_triangulated_dpstate[extrinsics1]")
if optimization_inputs_baseline.get('do_optimize_frames'):
istate_f0 = mrcal.state_index_frames(0, **optimization_inputs_baseline)
Nstate_frames = mrcal.num_states_frames(**optimization_inputs_baseline)
testutils.confirm_equal(dp_triangulated_dpstate[..., istate_f0:istate_f0 +
Nstate_frames],
nps.clump(dp_triangulated_drtrf_empirical, n=-2),
relative=True,
worstcase=True,
eps=0.1,
msg="Gradient check: dp_triangulated_drtrf")
# dp_triangulated_dq_empirical has shape (Npoints,3, Npoints,Ncameras,2)
# The cross terms (p_triangulated(point=A), q(point=B)) should all be zero
dp_triangulated_dq_empirical_cross_only = dp_triangulated_dq_empirical
dp_triangulated_dq_empirical = np.zeros((Npoints, 3, 2, 2), dtype=float)
dp_triangulated_dq = np.zeros((Npoints, 3, 2, 2), dtype=float)
dp_triangulated_dq_flattened = nps.clump(dp_triangulated_dq, n=-2)
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]
optimization_inputs['do_optimize_frames'] = True
optimization_inputs['do_optimize_calobject_warp'] = False
mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=True)
optimization_inputs['do_optimize_intrinsics_core'] = True
optimization_inputs['do_optimize_intrinsics_distortions'] = False
optimization_inputs['do_optimize_extrinsics'] = True
optimization_inputs['do_optimize_frames'] = True
optimization_inputs['do_optimize_calobject_warp'] = False
mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=True)
testutils.confirm_equal(mrcal.num_states_intrinsics(**optimization_inputs),
4 * Ncameras, "num_states_intrinsics()")
testutils.confirm_equal(mrcal.num_states_extrinsics(**optimization_inputs),
6 * (Ncameras - 1), "num_states_extrinsics()")
testutils.confirm_equal(mrcal.num_states_frames(**optimization_inputs),
6 * Nframes, "num_states_frames()")
testutils.confirm_equal(mrcal.num_states_points(**optimization_inputs), 0,
"num_states_points()")
testutils.confirm_equal(mrcal.num_states_calobject_warp(**optimization_inputs),
0, "num_states_calobject_warp()")
testutils.confirm_equal(
mrcal.num_measurements_boards(**optimization_inputs),
object_width_n * object_height_n * 2 * Nframes * Ncameras,
"num_measurements_boards()")
testutils.confirm_equal(mrcal.num_measurements_points(**optimization_inputs),
0, "num_measurements_points()")
testutils.confirm_equal(
mrcal.num_measurements_regularization(**optimization_inputs), Ncameras * 2,
"num_measurements_regularization()")