Esempio n. 1
0
              lensmodel                                 = lensmodel,
              calobject_warp                            = np.array((1e-3, 2e-3)),
              imagersizes                               = imagersizes,
              calibration_object_spacing                = 0.1,
              point_min_range                           = 1.0,
              point_max_range                           = 1000.0,
              verbose                                   = False,
              **kwargs )

    x, J = mrcal.optimizer_callback(**optimization_inputs)[1:3]
    J = J.toarray()

    # let's make sure that pack and unpack work correctly
    J2 = J.copy()
    mrcal.pack_state(J2, **optimization_inputs)
    mrcal.unpack_state(J2, **optimization_inputs)
    testutils.confirm_equal(J2, J, "unpack(pack(J)) = J")
    J2 = J.copy()
    mrcal.unpack_state(J2, **optimization_inputs)
    mrcal.pack_state(J2, **optimization_inputs)
    testutils.confirm_equal(J2, J, "pack(unpack(J)) = J")

    # I compare full-state J so that I can change SCALE_... without breaking the
    # test
    mrcal.pack_state(J, **optimization_inputs)

    if store_current_output_as_reference:
        np.save(f"{testdir}/data/test-optimizer-callback-ref-x-{itest}.npy", x)
        np.save(f"{testdir}/data/test-optimizer-callback-ref-J-{itest}.npy", J)
    else:
        x_ref = np.load(
Esempio n. 2
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]
Esempio n. 3
0
testutils.confirm_equal(dp_triangulated_dq,
                        dp_triangulated_dq_empirical,
                        relative = True,
                        worstcase = True,
                        eps = 1e-6,
                        msg = "Gradient check: dp_triangulated_dq")

Nmeasurements_observations = mrcal.num_measurements_boards(**optimization_inputs_baseline)
if Nmeasurements_observations == mrcal.num_measurements(**optimization_inputs_baseline):
    # Note the special-case where I'm using all the observations
    Nmeasurements_observations = None

# I look at the two triangulated points together. This is a (6,) vector. And I
# pack the denominator by unpacking the numerator
dp0p1_triangulated_dppacked = copy.deepcopy(dp_triangulated_dpstate)
mrcal.unpack_state(dp0p1_triangulated_dppacked, **optimization_inputs_baseline)
dp0p1_triangulated_dppacked = nps.clump(dp0p1_triangulated_dppacked,n=2)


# My input vector, whose noise I'm propagating, is x = [q_calibration
# q_triangulation]: the calibration-time pixel observations and the
# observation-time pixel observations. These are independent, so Var(x) is
# block-diagonal. I want to propagate the noise in x to some function f(x). As
# usual, Var(f) = df/dx Var(x) (df/dx)T. I have x = [qc qt] and a block-diagonal
# Var(x) so Var(f) = df/dqc Var(qc) (df/dqc)T + df/dqt Var(qt) (df/dqt)T. So I
# can treat the two noise contributions separately, and add the two variances
# together

# The variance due to calibration-time noise
Var_p0p1_triangulated = \
    mrcal.model_analysis._projection_uncertainty_make_output(factorization, Jpacked,