def plotoptions_state_boundaries(**optimization_inputs):
r'''Return the 'set' plot options for gnuplotlib to show the state boundaries
SYNOPSIS
import numpy as np
import gnuplotlib as gp
import mrcal
model = mrcal.cameramodel('xxx.cameramodel')
optimization_inputs = model.optimization_inputs()
J = mrcal.optimizer_callback(**optimization_inputs)[2]
gp.plot( np.sum(np.abs(J.toarray()), axis=-2),
_set = mrcal.plotoptions_state_boundaries(**optimization_inputs) )
# a plot pops up showing the magnitude of the effects of each element of the
# packed state (as seen by the optimizer), with boundaries between the
# different state variables denoted
When plotting the state vector (or anything relating to it, such as rows of the
Jacobian), it is usually very useful to infer at a glance the meaning of each
part of the plot. This function returns a list of 'set' directives passable to
gnuplotlib that show the boundaries inside the state vector.
ARGUMENTS
**optimization_inputs: a dict() of arguments passable to mrcal.optimize() and
mrcal.optimizer_callback(). These define the full optimization problem, and can
be obtained from the optimization_inputs() method of mrcal.cameramodel
RETURNED VALUE
A list of 'set' directives passable as plot options to gnuplotlib
'''
istate0 = []
try: istate0.append(mrcal.state_index_intrinsics (0, **optimization_inputs))
except: pass
try: istate0.append(mrcal.state_index_extrinsics (0, **optimization_inputs))
except: pass
try: istate0.append(mrcal.state_index_frames (0, **optimization_inputs))
except: pass
try: istate0.append(mrcal.state_index_points (0, **optimization_inputs))
except: pass
try: istate0.append(mrcal.state_index_calobject_warp( **optimization_inputs))
except: pass
return [f"arrow nohead from {x},graph 0 to {x},graph 1" for x in istate0]
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]
testutils.confirm_equal(dx_predicted,
dx_observed,
eps=1e-6,
worstcase=True,
msg="dx follows the prediction")
# The effect on the
# parameters should be dp = M dqref. Where M = inv(JtJ) Jobservationst W
M = np.linalg.solve(nps.matmult(nps.transpose(J0), J0),
nps.transpose(J0[:Nmeasurements_boards, :])) * w
dp_predicted = nps.matmult(dqref.ravel(), nps.transpose(M)).ravel()
istate0_frames = mrcal.state_index_frames(0, **baseline)
istate0_calobject_warp = mrcal.state_index_calobject_warp(**baseline)
istate0_extrinsics = mrcal.state_index_extrinsics(0, **baseline)
if istate0_extrinsics is None:
istate0_extrinsics = istate0_frames
slice_intrinsics = slice(0, istate0_extrinsics)
slice_extrinsics = slice(istate0_extrinsics, istate0_frames)
slice_frames = slice(istate0_frames, istate0_calobject_warp)
# These thresholds look terrible. And they are. But I'm pretty sure this is
# working properly. Look at the plots:
if 0:
import gnuplotlib as gp
plot_dp = gp.gnuplotlib(
title="dp predicted,observed",
_set=mrcal.plotoptions_state_boundaries(**optimization_inputs))
plot_dp.plot(
optimization_inputs['do_optimize_intrinsics_core'] = True
optimization_inputs['do_optimize_intrinsics_distortions'] = True
optimization_inputs['do_optimize_extrinsics'] = True
optimization_inputs['do_optimize_frames'] = True
optimization_inputs['do_optimize_calobject_warp'] = True
optimization_inputs['calobject_warp'] = np.array((0.001, 0.001))
stats = mrcal.optimize(**optimization_inputs, do_apply_outlier_rejection=True)
x = stats['x']
rmserr = stats['rms_reproj_error__pixels']
testutils.confirm_equal(mrcal.state_index_intrinsics(2, **optimization_inputs),
8 * 2, "state_index_intrinsics()")
testutils.confirm_equal(mrcal.state_index_extrinsics(2, **optimization_inputs),
8 * Ncameras + 6 * 2, "state_index_extrinsics()")
testutils.confirm_equal(mrcal.state_index_frames(2, **optimization_inputs),
8 * Ncameras + 6 * (Ncameras - 1) + 6 * 2,
"state_index_frames()")
testutils.confirm_equal(
mrcal.state_index_calobject_warp(**optimization_inputs),
8 * Ncameras + 6 * (Ncameras - 1) + 6 * Nframes,
"state_index_calobject_warp()")
testutils.confirm_equal(
mrcal.measurement_index_boards(2, **optimization_inputs),
object_width_n * object_height_n * 2 * 2, "measurement_index_boards()")
testutils.confirm_equal(
mrcal.measurement_index_regularization(**optimization_inputs),
object_width_n * object_height_n * 2 * Nframes * Ncameras,
# I store dp_triangulated_dp initialy, without worrying about the "packed" part.
# I'll scale the thing when done to pack it
dp_triangulated_dpstate = np.zeros((Npoints,3,Nstate), dtype=float)
istate_i0 = mrcal.state_index_intrinsics(0, **optimization_inputs_baseline)
istate_i1 = mrcal.state_index_intrinsics(1, **optimization_inputs_baseline)
# I'm expecting the layout of a vanilla calibration problem, and I assume that
# camera0 is at the reference below. Here I confirm that this assumption is
# correct
icam_extrinsics0 = mrcal.corresponding_icam_extrinsics(0, **optimization_inputs_baseline)
icam_extrinsics1 = mrcal.corresponding_icam_extrinsics(1, **optimization_inputs_baseline)
if not (icam_extrinsics0 < 0 and icam_extrinsics1 == 0):
raise Exception("Vanilla calibration problem expected, but got something else instead. Among others, _triangulate() assumes the triangulated result is in cam0, which is the same as the ref coord system")
istate_e1 = mrcal.state_index_extrinsics(icam_extrinsics1, **optimization_inputs_baseline)
istate_f0 = mrcal.state_index_frames(0, **optimization_inputs_baseline)
Nstate_frames = mrcal.num_states_frames(**optimization_inputs_baseline)
# dp_triangulated_di0 = dp_triangulated_dv0 dvlocal0_di0
# dp_triangulated_di1 = dp_triangulated_dv1 dv1_dvlocal1 dvlocal1_di1
nps.matmult( dp_triangulated_dv0,
dvlocal0_dintrinsics0,
out = dp_triangulated_dpstate[..., istate_i0:istate_i0+Nintrinsics])
nps.matmult( dp_triangulated_dv1,
dv1_dvlocal1,
dvlocal1_dintrinsics1,
out = dp_triangulated_dpstate[..., istate_i1:istate_i1+Nintrinsics])
# dp_triangulated_de0 doesn't exist: assuming vanilla calibration problem, so
