Exemplo n.º 1
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]
Exemplo n.º 2
0
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()")

optimization_inputs['do_optimize_intrinsics_core'] = True