model_analysis.py

#!/usr/bin/python3

# Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
# Government sponsorship acknowledged. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0

'''Routines for analysis of camera projection

This is largely dealing with uncertainty and projection diff operations.

All functions are exported into the mrcal module. So you can call these via
mrcal.model_analysis.fff() or mrcal.fff(). The latter is preferred.

'''

import numpy as np
import numpysane as nps
import sys
import re
import mrcal


def implied_Rt10__from_unprojections(q0, p0, v1,
                                     *,
                                     weights      = None,
                                     atinfinity   = True,
                                     focus_center = np.zeros((2,), dtype=float),
                                     focus_radius = 1.0e8):

    r'''Compute the implied-by-the-intrinsics transformation to fit two cameras' projections

SYNOPSIS

    models = ( mrcal.cameramodel('cam0-dance0.cameramodel'),
               mrcal.cameramodel('cam0-dance1.cameramodel') )

    lensmodels      = [model.intrinsics()[0] for model in models]
    intrinsics_data = [model.intrinsics()[1] for model in models]

    # v  shape (...,Ncameras,Nheight,Nwidth,...)
    # q0 shape (...,         Nheight,Nwidth,...)
    v,q0 = \
        mrcal.sample_imager_unproject(60, None,
                                      *models[0].imagersize(),
                                      lensmodels, intrinsics_data,
                                      normalize = True)
    implied_Rt10 = \
        mrcal.implied_Rt10__from_unprojections(q0, v[0,...], v[1,...])

    q1 = mrcal.project( mrcal.transform_point_Rt(implied_Rt10, v[0,...]),
                        *models[1].intrinsics())

    projection_diff = q1 - q0

When comparing projections from two lens models, it is usually necessary to
align the geometry of the two cameras, to cancel out any transformations implied
by the intrinsics of the lenses. This transformation is computed by this
function, used primarily by mrcal.show_projection_diff() and the
mrcal-show-projection-diff tool.

What are we comparing? We project the same world point into the two cameras, and
report the difference in projection. Usually, the lens intrinsics differ a bit,
and the implied origin of the camera coordinate systems and their orientation
differ also. These geometric uncertainties are baked into the intrinsics. So
when we project "the same world point" we must apply a geometric transformation
to compensate for the difference in the geometry of the two cameras. This
transformation is unknown, but we can estimate it by fitting projections across
the imager: the "right" transformation would result in apparent low projection
diffs in a wide area.

The primary inputs are unprojected gridded samples of the two imagers, obtained
with something like mrcal.sample_imager_unproject(). We grid the two imagers,
and produce normalized observation vectors for each grid point. We pass the
pixel grid from camera0 in q0, and the two unprojections in p0, v1. This
function then tries to find a transformation to minimize

  norm2( project(camera1, transform(p0)) - q1 )

We return an Rt transformation to map points in the camera0 coordinate system to
the camera1 coordinate system. Some details about this general formulation are
significant:

- The subset of points we use for the optimization
- What kind of transformation we use

In most practical usages, we would not expect a good fit everywhere in the
imager: areas where no chessboards were observed will not fit well, for
instance. From the point of view of the fit we perform, those ill-fitting areas
should be treated as outliers, and they should NOT be a part of the solve. How
do we specify the well-fitting area? The best way is to use the model
uncertainties to pass the weights in the "weights" argument (see
show_projection_diff() for an implementation). If uncertainties aren't
available, or if we want a faster solve, the focus region can be passed in the
focus_center, focus_radius arguments. By default, these are set to encompass the
whole imager, since the uncertainties would take care of everything, but without
uncertainties (weights = None), these should be set more discriminately. It is
possible to pass both a focus region and weights, but it's probably not very
useful.

Unlike the projection operation, the diff operation is NOT invariant under
geometric scaling: if we look at the projection difference for two points at
different locations along a single observation ray, there will be a variation in
the observed diff. This is due to the geometric difference in the two cameras.
If the models differed only in their intrinsics parameters, then this would not
happen. Thus this function needs to know how far from the camera it should look.
By default (atinfinity = True) we look out to infinity. In this case, p0 is
expected to contain unit vectors. To use any other distance, pass atinfinity =
False, and pass POINTS in p0 instead of just observation directions. v1 should
always be normalized. Generally the most confident distance will be where the
chessboards were observed at calibration time.

Practically, it is very easy for the unprojection operation to produce nan or
inf values. And the weights could potentially have some invalid values also.
This function explicitly checks for such illegal data in p0, v1 and weights, and
ignores those points.

ARGUMENTS

- q0: an array of shape (Nh,Nw,2). Gridded pixel coordinates covering the imager
  of both cameras

- p0: an array of shape (...,Nh,Nw,3). An unprojection of q0 from camera 0. If
  atinfinity, this should contain unit vectors, else it should contain points in
  space at the desired distance from the camera. This array may have leading
  dimensions that are all used in the fit. These leading dimensions correspond
  to those in the "weights" array

- v1: an array of shape (Nh,Nw,3). An unprojection of q0 from camera 1. This
  should always contain unit vectors, regardless of the value of atinfinity

- weights: optional array of shape (...,Nh,Nw); None by default. If given, these
  are used to weigh each fitted point differently. Usually we use the projection
  uncertainties to apply a stronger weight to more confident points. If omitted
  or None, we weigh each point equally. This array may have leading dimensions
  that are all used in the fit. These leading dimensions correspond to those in
  the "p0" array

- atinfinity: optional boolean; True by default. If True, we're looking out to
  infinity, and I compute a rotation-only fit; a full Rt transformation is still
  returned, but Rt[3,:] is 0; p0 should contain unit vectors. If False, I'm
  looking out to a finite distance, and p0 should contain 3D points specifying
  the positions of interest.

- focus_center: optional array of shape (2,); (0,0) by default. Used to indicate
  that we're interested only in a subset of pixels q0, a distance focus_radius
  from focus_center. By default focus_radius is LARGE, so we use all the points.
  This is intended to be used if no uncertainties are available, and we need to
  manually select the focus region.

- focus_radius: optional value; LARGE by default. Used to indicate that we're
  interested only in a subset of pixels q0, a distance focus_radius from
  focus_center. By default focus_radius is LARGE, so we use all the points. This
  is intended to be used if no uncertainties are available, and we need to
  manually select the focus region.

RETURNED VALUE

An array of shape (4,3), representing an Rt transformation from camera0 to
camera1. If atinfinity then we're computing a rotation-fit only, but we still
report a full Rt transformation with the t component set to 0

    '''

    # This is very similar in spirit to what compute_Rcorrected_dq_dintrinsics() did
    # (removed in commit 4240260), but that function worked analytically, while this
    # one explicitly computes the rotation by matching up known vectors.

    import scipy.optimize

    ### flatten all the input arrays
    # shape (N,2)
    q0 = nps.clump(q0, n=q0.ndim-1)
    # shape (M,N,3)
    p0 = nps.transpose(nps.clump(nps.mv( nps.atleast_dims(p0, -3),
                                         -1,-3),
                                 n=-2))
    # shape (N,3)
    v1 = nps.clump(v1, n=v1.ndim-1)

    if weights is None:
        weights = np.ones(p0.shape[:-1], dtype=float)
    else:
        # shape (..., Nh,Nw) -> (M,N,) where N = Nh*Nw
        weights = nps.clump( nps.clump(weights, n=-2),
                             n = weights.ndim-2)

        # Any inf/nan weight or vector are set to 0
        weights = weights.copy()
        weights[ ~np.isfinite(weights) ] = 0.0

    p0 = p0.copy()
    v1 = v1.copy()

    # p0 had shape (N,3). Collapse all the leading dimensions into one
    # And do the same for weights

    i_nan_p0 = ~np.isfinite(p0)
    p0[i_nan_p0] = 0.
    weights[i_nan_p0[...,0]] = 0.0
    weights[i_nan_p0[...,1]] = 0.0
    weights[i_nan_p0[...,2]] = 0.0

    i_nan_v1 = ~np.isfinite(v1)
    v1[i_nan_v1] = 0.
    weights[..., i_nan_v1[...,0]] = 0.0
    weights[..., i_nan_v1[...,1]] = 0.0
    weights[..., i_nan_v1[...,2]] = 0.0

    # We try to match the geometry in a particular region
    q_off_center = q0 - focus_center
    i = nps.norm2(q_off_center) < focus_radius*focus_radius
    if np.count_nonzero(i)<3:
        raise Exception("Focus region contained too few points")

    p0_cut  = p0     [..., i, :]
    v1_cut  = v1     [     i, :]
    weights = weights[..., i   ]

    def residual_jacobian_rt(rt):

        # rtp0      has shape (M,N,3)
        # drtp0_drt has shape (M,N,3,6)
        rtp0, drtp0_drt, _ = \
            mrcal.transform_point_rt(rt, p0_cut,
                                     get_gradients = True)

        # inner(a,b)/(mag(a)*mag(b)) = cos(x) ~ 1 - x^2/2
        # Each of these has shape (M,N,)
        mag_rtp0 = nps.mag(rtp0)
        inner    = nps.inner(rtp0, v1_cut)
        th2      = 2.* (1.0 - inner / mag_rtp0)
        x        = th2 * weights

        # shape (M,N,6)
        dmag_rtp0_drt = nps.matmult( nps.dummy(rtp0, -2),   # shape (M,N,1,3)
                                     drtp0_drt              # shape (M,N,3,6)
                                     # matmult has shape (M,N,1,6)
                                   )[...,0,:] / \
                                   nps.dummy(mag_rtp0, -1)  # shape (M,N,1)
        # shape (M,N,6)
        dinner_drt    = nps.matmult( nps.dummy(v1_cut, -2), # shape (M,N,1,3)
                                     drtp0_drt              # shape (M,N,3,6)
                                     # matmult has shape (M,N,1,6)
                                   )[...,0,:]

        # dth2 = 2 (inner dmag_rtp0 - dinner mag_rtp0)/ mag_rtp0^2
        # shape (M,N,6)
        J = 2. * \
            (nps.dummy(inner,    -1) * dmag_rtp0_drt - \
             nps.dummy(mag_rtp0, -1) * dinner_drt) / \
             nps.dummy(mag_rtp0*mag_rtp0, -1) * \
             nps.dummy(weights,-1)
        return x.ravel(), nps.clump(J, n=J.ndim-1)


    def residual_jacobian_r(r):

        # rp0     has shape (M,N,3)
        # drp0_dr has shape (M,N,3,3)
        rp0, drp0_dr, _ = \
            mrcal.rotate_point_r(r, p0_cut,
                                 get_gradients = True)

        # inner(a,b)/(mag(a)*mag(b)) ~ cos(x) ~ 1 - x^2/2
        # Each of these has shape (M,N)
        inner = nps.inner(rp0, v1_cut)
        th2   = 2.* (1.0 - inner)
        x     = th2 * weights

        # shape (M,N,3)
        dinner_dr = nps.matmult( nps.dummy(v1_cut, -2), # shape (M,N,1,3)
                                 drp0_dr                # shape (M,N,3,3)
                                 # matmult has shape (M,N,1,3)
                               )[...,0,:]

        J = -2. * dinner_dr * nps.dummy(weights,-1)
        return x.ravel(), nps.clump(J, n=J.ndim-1)


    cache = {'rt': None}
    def residual(rt, f):
        if cache['rt'] is None or not np.array_equal(rt,cache['rt']):
            cache['rt'] = rt
            cache['x'],cache['J'] = f(rt)
        return cache['x']
    def jacobian(rt, f):
        if cache['rt'] is None or not np.array_equal(rt,cache['rt']):
            cache['rt'] = rt
            cache['x'],cache['J'] = f(rt)
        return cache['J']


    # # gradient check
    # import gnuplotlib as gp
    # rt0 = np.random.random(6)*1e-3
    # x0,J0 = residual_jacobian_rt(rt0)
    # drt = np.random.random(6)*1e-7
    # rt1 = rt0+drt
    # x1,J1 = residual_jacobian_rt(rt1)
    # dx_theory = nps.matmult(J0, nps.transpose(drt)).ravel()
    # dx_got    = x1-x0
    # relerr = (dx_theory-dx_got) / ( (np.abs(dx_theory)+np.abs(dx_got))/2. )
    # gp.plot(relerr, wait=1, title='rt')
    # r0 = np.random.random(3)*1e-3
    # x0,J0 = residual_jacobian_r(r0)
    # dr = np.random.random(3)*1e-7
    # r1 = r0+dr
    # x1,J1 = residual_jacobian_r(r1)
    # dx_theory = nps.matmult(J0, nps.transpose(dr)).ravel()
    # dx_got    = x1-x0
    # relerr = (dx_theory-dx_got) / ( (np.abs(dx_theory)+np.abs(dx_got))/2. )
    # gp.plot(relerr, wait=1, title='r')
    # sys.exit()


    # I was using loss='soft_l1', but it behaved strangely. For large
    # f_scale_deg it should be equivalent to loss='linear', but I was seeing
    # large diffs when comparing a model to itself:
    #
    #   ./mrcal-show-projection-diff --gridn 50 28 test/data/cam0.splined.cameramodel{,} --distance 3
    #
    # f_scale_deg needs to be > 0.1 to make test-projection-diff.py pass, so
    # there was an uncomfortably-small usable gap for f_scale_deg. loss='huber'
    # should work similar-ish to 'soft_l1', and it works even for high
    # f_scale_deg
    f_scale_deg = 5
    loss        = 'huber'

    if atinfinity:


        # This is similar to a basic procrustes fit, but here we're using an L1
        # cost function
        r = np.random.random(3) * 1e-5

        res = scipy.optimize.least_squares(residual,
                                           r,
                                           jac=jacobian,
                                           method='dogbox',

                                           loss=loss,
                                           f_scale = (f_scale_deg * np.pi/180.)**2.,
                                           # max_nfev=1,
                                           args=(residual_jacobian_r,),

                                           # Without this, the optimization was
                                           # ending too quickly, and I was
                                           # seeing not-quite-optimal solutions.
                                           # Especially for
                                           # very-nearly-identical rotations.
                                           # This is tested by diffing the same
                                           # model in test-projection-diff.py.
                                           # I'd like to set this to None to
                                           # disable the comparison entirely,
                                           # but that requires scipy >= 1.3.0.
                                           # So instead I set the threshold so
                                           # low that it's effectively disabled
                                           gtol = np.finfo(float).eps,
                                           verbose=0)
        Rt = np.zeros((4,3), dtype=float)
        Rt[:3,:] = mrcal.R_from_r(res.x)
        return Rt

    else:

        rt = np.random.random(6) * 1e-5

        res = scipy.optimize.least_squares(residual,
                                           rt,
                                           jac=jacobian,
                                           method='dogbox',

                                           loss=loss,
                                           f_scale = (f_scale_deg * np.pi/180.)**2.,
                                           # max_nfev=1,
                                           args=(residual_jacobian_rt,),

                                           # Without this, the optimization was
                                           # ending too quickly, and I was
                                           # seeing not-quite-optimal solutions.
                                           # Especially for
                                           # very-nearly-identical rotations.
                                           # This is tested by diffing the same
                                           # model in test-projection-diff.py.
                                           # I'd like to set this to None to
                                           # disable the comparison entirely,
                                           # but that requires scipy >= 1.3.0.
                                           # So instead I set the threshold so
                                           # low that it's effectively disabled
                                           gtol = np.finfo(float).eps )
        return mrcal.Rt_from_rt(res.x)


def worst_direction_stdev(cov):
    r'''Compute the worst-direction standard deviation from a NxN covariance matrix

SYNOPSIS

    # A covariance matrix
    print(cov)
    ===>
    [[ 1.  -0.4]
     [-0.4  0.5]]

    # Sample 1000 0-mean points using this covariance
    x = np.random.multivariate_normal(mean = np.array((0,0)),
                                      cov  = cov,
                                      size = (1000,))

    # Compute the worst-direction standard deviation of the sampled data
    print(np.sqrt(np.max(np.linalg.eig(np.mean(nps.outer(x,x),axis=0))[0])))
    ===>
    1.1102510878087053

    # The predicted worst-direction standard deviation
    print(mrcal.worst_direction_stdev(cov))
    ===> 1.105304960905736

The covariance of a (N,) random vector can be described by a (N,N)
positive-definite symmetric matrix. The 1-sigma contour of this random variable
is described by an ellipse with its axes aligned with the eigenvectors of the
covariance, and the semi-major and semi-minor axis lengths specified as the sqrt
of the corresponding eigenvalues. This function returns the worst-case standard
deviation of the given covariance: the sqrt of the largest eigenvalue.

Given the common case of a 2x2 covariance this function computes the result
directly. Otherwise it uses the numpy functions to compute the biggest
eigenvalue.

This function supports broadcasting fully.

DERIVATION

I solve this directly for the 2x2 case.

Let cov = (a b). If l is an eigenvalue of the covariance then
          (b c)

    (a-l)*(c-l) - b^2 = 0 --> l^2 - (a+c) l + ac-b^2 = 0

    --> l = (a+c +- sqrt( a^2 + 2ac + c^2 - 4ac + 4b^2)) / 2 =
          = (a+c +- sqrt( a^2 - 2ac + c^2 + 4b^2)) / 2 =
          = (a+c)/2 +- sqrt( (a-c)^2/4 + b^2)

So the worst-direction standard deviation is

    sqrt((a+c)/2 + sqrt( (a-c)^2/4 + b^2))

ARGUMENTS

- cov: the covariance matrices given as a (..., 2,2) array. Valid covariances
  are positive-semi-definite (symmetric with eigenvalues >= 0), but this is not
  checked

RETURNED VALUES

The worst-direction standard deviation. This is a scalar or an array, if we're
broadcasting

    '''

    cov = nps.atleast_dims(cov,-2)

    if cov.shape[-2:] == (1,1):
        return np.sqrt(cov[...,0,0])

    if cov.shape[-2:] == (2,2):
        a = cov[..., 0,0]
        b = cov[..., 1,0]
        c = cov[..., 1,1]
        return np.sqrt((a+c)/2 + np.sqrt( (a-c)*(a-c)/4 + b*b))

    if cov.shape[-1] != cov.shape[-2]:
        raise Exception(f"covariance matrices must be square. Got cov.shape = {cov.shape}")

    import scipy.sparse.linalg
    @nps.broadcast_define( (('N','N',),),
                           () )
    def largest_eigenvalue(V):
        return \
            scipy.sparse.linalg.eigsh(V, 1,
                                      which               = 'LM',
                                      return_eigenvectors = False)[0]
    return np.sqrt(largest_eigenvalue(cov))


def _observed_pixel_uncertainty_from_inputs(optimization_inputs,
                                            x = None):

    r'''Estimate the input noise from the solve residuals

Documented here:

  https://mrcal.secretsauce.net/formulation.html#estimating-input-noise

    '''

    if x is None:
        x = mrcal.optimizer_callback(**optimization_inputs,
                                     no_jacobian      = True,
                                     no_factorization = True)[1]

    sum_of_squares_measurements = 0
    Nmeasurements               = 0

    # shape (Nobservations*2)
    measurements = mrcal.measurements_board(optimization_inputs, x = x).ravel()
    if measurements.size:
        sum_of_squares_measurements += np.sum(measurements*measurements)
        Nmeasurements += measurements.size

    measurements = mrcal.measurements_point(optimization_inputs, x = x).ravel()
    if measurements.size:
        sum_of_squares_measurements += np.sum(measurements*measurements)
        Nmeasurements += measurements.size

    if Nmeasurements == 0:
        raise Exception("observed_pixel_uncertainty cannot be computed because we don't have any board or point observations")


    Nstate = mrcal.num_states(**optimization_inputs)

    # The main takeaway from the docs
    #   https://mrcal.secretsauce.net/formulation.html#estimating-input-noise
    # is:
    # norm2(x) ~ (Nmeas - Nstate + tr(Jreg inv_JtJ Jreg^T ) ) sigma^2
    #
    # RMS(x) = sqrt(norm2(x)/Nmeas)
    #        = sigma sqrt(Nmeas - Nstate + tr(Jreg inv_JtJ Jreg^T ) / Nmeas)
    #        = sigma sqrt(1 - Nstate/Nmeas + tr(Jreg inv_JtJ Jreg^T ) / Nmeas)
    #
    # So the correction factor is sqrt(1 - Nstate/Nmeas + tr(Jreg inv_JtJ Jreg^T ) / Nmeas)
    # I generally ignore the regularization
    if True:
        f = np.sqrt(1 - Nstate/Nmeasurements)
    else:
        # But as an experiment, I want to be able to include it. It should
        # mostly not make any difference
        Nmeasurements_regularization = mrcal.num_measurements_regularization(**optimization_inputs)
        if Nmeasurements_regularization == 0:
            f = np.sqrt(1 - Nstate/Nmeasurements)
        else:
            _,x,Jpacked,factorization = mrcal.optimizer_callback(**optimization_inputs)

            # I assume the regularization measurements are at the end
            # tr(Jreg inv_JtJ Jreg^T ) = tr(Jreg^T Jreg inv_JtJ )
            Jreg = Jpacked[-Nmeasurements_regularization:]
            JregtJreg = (Jreg.T @ Jreg).toarray().T
            f_extra = np.linalg.trace( factorization.solve_xt_JtJ_bt(JregtJreg) )
            f = np.sqrt(1 + (- Nstate + f_extra)/Nmeasurements)

    return np.sqrt(sum_of_squares_measurements / Nmeasurements) / f



def _propagate_calibration_uncertainty( what,
                                        *,

                                        # One of these must be given. If it's
                                        # dF_dbunpacked, then
                                        # optimization_inputs must be given too
                                        dF_dbpacked                        = None,
                                        dF_dbunpacked                      = None,
                                        # These are partly optional. I need
                                        # everything except optimization_inputs.
                                        # If any of the non-optimization_inputs
                                        # arguments are missing I need
                                        # optimization_inputs to compute them.
                                        x                                  = None,
                                        factorization                      = None,
                                        Jpacked                            = None,
                                        Nmeasurements_observations_leading = None,
                                        observed_pixel_uncertainty         = None,
                                        # can compute each of the above
                                        optimization_inputs                = None):
    r'''Helper for uncertainty propagation functions

Propagates the calibration-time uncertainty to compute Var(F) for some arbitrary
vector F. The user specifies the gradient dF/db: the sensitivity of F to noise
in the calibration state. The vector F can have any length: this is inferred
from the dimensions of the given dF/db gradient.

The given factorization uses the packed, unitless state: b*.

The given Jpacked uses the packed, unitless state: b*. Jpacked applies to all
observations.

The leading Nmeasurements_observations_leading rows apply to the observations of
the calibration object, and we use just those for the input noise propagation.
If Nmeasurements_observations_leading is None: we auto-detect this; this is the
default. If Nmeasurements_observations_leading==0: assume that ALL the
measurements come from the calibration object observations; a simplifed
expression can be used in this case. This produces incorrect results today:

  test/test-projection-uncertainty.py \
    --fixed cam0 \
    --model opencv4 \
    --do-sample \
    --reproject-perturbed cross-reprojection-rrp-Jfp \
    --observed-pixel-uncertainty 0.03 \
    --Nsamples 400 \
    --Ncameras 2 \
    --points

The given dF_dbpacked uses the packed, unitless state b*, so it already includes
the multiplication by D in the expressions below. It's usually sparse, but
stored densely.

The uncertainty computation in
https://mrcal.secretsauce.net/uncertainty.html concludes that

  Var(b*) = observed_pixel_uncertainty^2 inv(J*tJ*) J*[observations]t J*[observations] inv(J*tJ*)

Where b* and J* are the UNITLESS, packed state and the jacobian respectively:

  b = D b*
  J = J* inv(D)

In the special case where all the measurements come from observations, this
simplifies to

  Var(b*) = observed_pixel_uncertainty^2 inv(J*tJ*)

My factorization is of packed (scaled, unitless) flavors of J (J*). So

  Var(b) = D Var(b*) D

I want Var(F) = dF/db Var(b) dF/dbt

So

  Var(F) = dF/db D Var(b*) D dF/dbt

In the regularized case I have

  Var(F) = dF/db D inv(J*tJ*) J*[observations]t J*[observations] inv(J*tJ*) D dF/dbt observed_pixel_uncertainty^2

I have

  J* = [ J*[o] ]
       [ J*[r] ]

where J*[r] has MANY FEWER rows than J*[o]. J*tJ* = J*[o]tJ*[o] + J*[r]tJ*[r], so

  J*[o]tJ*[o] = J*tJ* - J*[r]tJ*[r]

  Var(F) = dF/db D inv(J*tJ*) (J*tJ* - J*[r]tJ*[r]) inv(J*tJ*) D dF/dbt observed_pixel_uncertainty^2
         = dF/db D inv(J*tJ*) D dF/dbt observed_pixel_uncertainty^2 -
           dF/db D inv(J*tJ*) J*[r]tJ*[r] inv(J*tJ*) D dF/dbt observed_pixel_uncertainty^2

It is more efficient to compute inv(J*tJ*) D dF/dbt than inv(J*tJ*) J*[r]t:
there's less to compute, and the matrices are smaller. Thus I don't compute the
covariances directly.

In the non-regularized case:

  Var(F) = dF/db D inv(J*tJ*) D dF/dbt

  1. dF/db D inv(J*tJ*) = solve( J*tJ*, D dF/dbt)
     The result has shape (Nstate,len(F))

  2. pre-multiply by dF/db D

  3. multiply by observed_pixel_uncertainty^2

In the regularized case:

  Var(F) = dF/db D inv(J*tJ*) J*[observations]t J*[observations] inv(J*tJ*) D dF/dbt

  1. solve( J*tJ*, D dF/dbt)
     The result has shape (Nstate,len(F))

  2. Pre-multiply by J*[observations]
     The result has shape (Nmeasurements_observations_leading,2)

  3. Compute the sum of the outer products of each row

  4. multiply by observed_pixel_uncertainty^2

    '''

    what_known = set(('covariance', 'worstdirection-stdev', 'rms-stdev', '_covariance-raw'))
    if not what in what_known:
        raise Exception(f"'what' kwarg must be in {what_known}, but got '{what}'")

    if dF_dbpacked   is None and \
       dF_dbunpacked is None:
        raise Exception("Exactly one of dF_dbpacked,dF_dbunpacked must be given")
    if dF_dbpacked   is not None and \
       dF_dbunpacked is not None:
        raise Exception("Exactly one of dF_dbpacked,dF_dbunpacked must be given")

    if dF_dbunpacked is not None:
        if optimization_inputs is None:
            raise Exception('dF_dbunpacked is given but optimization_inputs is not. Either pass dF_dbpacked or pass optimization_inputs in as well')

        # Make dF_db use the packed state. I call "unpack_state" because the
        # state is in the denominator
        dF_dbpacked = np.array(dF_dbunpacked) # make a copy
        mrcal.unpack_state(dF_dbpacked, **optimization_inputs)

    if \
       x                                  is None or \
       factorization                      is None or \
       Jpacked                            is None or \
       Nmeasurements_observations_leading is None or \
       observed_pixel_uncertainty         is None:
        if optimization_inputs is None:
            raise Exception("At least one of (factorization,Jpacked,Nmeasurements_observations_leading,observed_pixel_uncertainty) are None, so optimization_inputs MUST have been given to compute them")

    if factorization is None or Jpacked is None or x is None:
        _,x,Jpacked,factorization = mrcal.optimizer_callback(**optimization_inputs)
        if factorization is None:
            raise Exception("Cannot compute the uncertainty: factorization computation failed")


    if Nmeasurements_observations_leading is None:
        Nmeasurements_boards         = mrcal.num_measurements_boards(**optimization_inputs)
        Nmeasurements_points         = mrcal.num_measurements_points(**optimization_inputs)
        Nmeasurements_regularization = mrcal.num_measurements_regularization(**optimization_inputs)
        Nmeasurements_all            = mrcal.num_measurements(**optimization_inputs)
        imeas_regularization         = mrcal.measurement_index_regularization(**optimization_inputs)
        if Nmeasurements_boards + \
           Nmeasurements_points + \
           Nmeasurements_regularization != \
               Nmeasurements_all:
            raise Exception("Some measurements other than boards, points and regularization are present. Don't know what to do")
        if imeas_regularization is not None and \
           imeas_regularization + Nmeasurements_regularization != Nmeasurements_all:
            raise Exception("Regularization measurements are NOT at the end. Don't know what to do")

        if Nmeasurements_regularization == 0:
            # Note the special-case where I'm using all the observations. No other
            # measurements are present other than the chessboard observations
            Nmeasurements_observations_leading = 0
        else:
            Nmeasurements_observations_leading = \
                Nmeasurements_all - Nmeasurements_regularization
            if Nmeasurements_observations_leading == 0:
                raise Exception("No non-regularization measurements. Don't know what to do")

    if observed_pixel_uncertainty is None:
        observed_pixel_uncertainty = _observed_pixel_uncertainty_from_inputs(optimization_inputs,
                                                                             x = x)

    def process_slice(dF_dbpacked):
        if Nmeasurements_observations_leading > 0:
            # I have regularization. Use the more complicated expression

            # shape (N,Nstate) where N=2 usually
            A = factorization.solve_xt_JtJ_bt( dF_dbpacked )

            # I see no python way to do matrix multiplication with sparse matrices,
            # so I have my own routine in C. AND the C routine does the outer
            # product, so there's no big temporary expression. It's much faster
            if A.ndim >= 2 and A.shape[-2] == 2:
                f = mrcal._mrcal_npsp._A_Jt_J_At__2
            else:
                f = mrcal._mrcal_npsp._A_Jt_J_At
            return \
                f(A, Jpacked.indptr, Jpacked.indices, Jpacked.data,
                  Nleading_rows_J = Nmeasurements_observations_leading)
        else:
            # No regularization. Use the simplified expression

            # The expression I had earlier. Works properly, but is slow:
            # # time ./mrcal-show-projection-uncertainty --gridn 120 90 --hardcopy /tmp/tst.gp **/*.cameramodel(OL[1])
            # # 21.48s user 0.27s system 99% cpu 21.750 total
            #
            # # shape (N,Nstate) where N=2 usually
            # A = factorization.solve_xt_JtJ_bt( dF_dbpacked )
            # Var_dF = nps.matmult(dF_dbpacked, nps.transpose(A))
            #
            # Instead, I do something smarter. I need to compute
            #
            #   sigma^2 dF/db D inv(J*tJ*) D dF/dbt
            #
            # I just computed a factorization, so I have
            #
            #   J*tJ* = L Lt
            #
            # so I can equivalently compute
            #
            #   sigma^2 norm2( inv(L) D dF/dbt )
            #
            # when cholmod_solve2() tries to solve inv(JtJ) x = b, it solves two
            # linear systems in series: one with L and then again with Lt. This new
            # expression runs only one solve, which speeds things up dramatically.

            # I tried several ways to compute this. All produce the same result, but
            # have different speeds. By default cholmod gives me an LDLt
            # factorization, not an LLt factorization (this is a different D), so I
            # need to handle this extra D in some way.

            # Slowest; two different solves.
            # time ./mrcal-show-projection-uncertainty --gridn 120 90 --hardcopy /tmp/tst.gp **/*.cameramodel(OL[1])
            # 20.64s user 1.30s system 99% cpu 21.938 total
            #
            # A1 = factorization.solve_xt_JtJ_bt( dF_dbpacked, sys='P' )
            # A2 = factorization.solve_xt_JtJ_bt( A1,          sys='L' )
            # A3 = factorization.solve_xt_JtJ_bt( A1,          sys='LD' )
            # Var_dF = nps.matmult(A2, nps.transpose(A3))
            #

            # Fastest, but works only if I have an LLt factorization. Can be
            # requested with this patch:
            #
            #   diff --git a/mrcal-pywrap.c b/mrcal-pywrap.c
            #   index b0d45fcc..b7c87090 100644
            #   --- a/mrcal-pywrap.c
            #   +++ b/mrcal-pywrap.c
            #   @@ -290,3 +290,5 @@
            #            self->common.supernodal = 0;
            #
            #   +        // self->common.final_ll = 1;
            #   +
            #            // I want all output to go to STDERR, not STDOUT
            #
            # I don't know if there are downsides to this, so I don't do this.
            #
            # time ./mrcal-show-projection-uncertainty --gridn 120 90 --hardcopy /tmp/tst.gp **/*.cameramodel(OL[1])
            # 11.80s user 0.38s system 99% cpu 12.187 total
            #
            # A1 = factorization.solve_xt_JtJ_bt( dF_dbpacked, sys='P' )
            # A2 = factorization.solve_xt_JtJ_bt( A1,          sys='L' )
            # Var_dF = nps.matmult(A2, nps.transpose(A2))

            # A bit slower, uses more memory, but works with LDLt. This is what I
            # use
            #
            # time ./mrcal-show-projection-uncertainty --gridn 120 90 --hardcopy /tmp/tst.gp **/*.cameramodel(OL[1])
            # 12.29s user 0.96s system 99% cpu 13.253 total
            A1 = factorization.solve_xt_JtJ_bt( dF_dbpacked, sys='P' )
            del dF_dbpacked
            A2 = factorization.solve_xt_JtJ_bt( A1,          sys='L' )
            del A1
            A3 = factorization.solve_xt_JtJ_bt( A2,          sys='D' )
            return \
                nps.matmult(A2, nps.transpose(A3))




    scalar = (dF_dbpacked.ndim == 1)

    Var_dF = process_slice(dF_dbpacked)
    if what == '_covariance-raw':
        return Var_dF,observed_pixel_uncertainty

    if what == 'covariance':
        if scalar:
            Var_dF = Var_dF[0,0]
        return Var_dF * observed_pixel_uncertainty*observed_pixel_uncertainty
    if what == 'worstdirection-stdev':
        return worst_direction_stdev(Var_dF) * observed_pixel_uncertainty
    if what == 'rms-stdev':
        # Compute the RMS of the standard deviations in each direction
        # RMS(stdev) =
        # = sqrt( mean(stdev^2) )
        # = sqrt( mean(var) )
        # = sqrt( sum(var)/N )
        # = sqrt( trace/N )
        return np.sqrt(nps.trace(Var_dF)/Var_dF.shape[-1]) * observed_pixel_uncertainty
    else:
        raise Exception("Shouldn't have gotten here. There's a bug")



def _dq_db__Kunpacked_rrp(## write output here
                          # shape (..., 2,Nstate)
                          dq_db,
                          ## inputs
                          # shape (..., Ncameras_extrinsics, 3)
                          p_ref,
                          # shape (..., 2,3)
                          dq_dpcam,
                          # shape (..., Ncameras_extrinsics,3,3)
                          dpcam_dpref,
                          # shape (6,Nstate)
                          Kunpacked_rrp,
                          *,
                          atinfinity):

    # atinfinity = rotation-only
    # K is drt_ref_refperturbed/db


    # From https://mrcal.secretsauce.net/docs-3.0/uncertainty-cross-reprojection.html:
    #   q*    = project( pcam*, intrinsics* )
    #   pcam* = Tc*r* pref*
    #   pref* = Tr*r  pref
    #
    # So q* depends on Tr*r, Tc*,r*, intrinsics*. The extrinsics and intrinsics
    # dependence was handled by the caller. Here I ingest only the dependence on
    # Tr*r
    Ncameras_extrinsics = p_ref.shape[-2]
    if Ncameras_extrinsics != 1:
        raise Exception("I only handle stationary cameras for now")
    # shape (..., 3)
    p_ref = p_ref[...,0,:]
    # shape (..., 3,3)
    dpcam_dpref = dpcam_dpref[...,0,:,:]

    # So
    #   dq/db[frame_all,calobject_warp] =
    #     dq_dpcam dpcam__dpref dpref*__drt_ref_ref* Kunpacked_rrp
    if dpcam_dpref is not None:
        dq_dpref = nps.matmult(dq_dpcam, dpcam_dpref)
    else:
        dq_dpref = dq_dpcam

    # pref = transform(rt_ref_ref*, pref*)
    #   where rt_ref_ref* is tiny
    # -> pref ~ pref* + cross(r, pref) + t
    #         = pref* - cross(pref, r) + t
    # -> 0 = dpref*/dr - skew(pref)
    #    0 = dpref*/dt + I
    # -> dpref*/dr = skew(pref)
    #    dpref*/dt = -I
    dprefp__dr_ref_refp = mrcal.skew_symmetric(p_ref)

    # I don't explicitly store dpref*/dt. I multiply by I implicitly

    dq__dr_ref_refp = nps.matmult(dq_dpref, dprefp__dr_ref_refp)

    # I apply this to the whole dq_db array. Kunpacked_rrp has 0 rows for
    # the unaffected state, so the columns that should be untouched will
    # be untouched
    dq_db += nps.matmult(dq__dr_ref_refp, Kunpacked_rrp[:3,:])

    if not atinfinity:
        dq_db -= nps.matmult(dq_dpref,        Kunpacked_rrp[3:,:])


def _dq_db__projection_uncertainty( # shape (...,3)
                                    p_cam,
                                    lensmodel,

                                    # The intrinsics for THIS camera. If we
                                    # optimize any of it, it's
                                    # intrinsics_data[slice_intrinsics_arg].
                                    # Partial optimization is possible:
                                    # do_optimize_intrinsics_core can be on/off
                                    intrinsics_data,

                                    # The block of extrinsics for this camera.
                                    rt_cam_ref,

                                    # The block of frame poses for this camera.
                                    # We optimize all of this, or nothing,
                                    # depending on whether istate_frames0 is
                                    # None
                                    rt_ref_frame,
                                    Nstate,

                                    slice_intrinsics_state,
                                    slice_intrinsics_arg,

                                    slice_extrinsics_state,
                                    istate_frames0,
                                    *,
                                    # shape (6,Nstate)
                                    Kunpacked_cross_reprojection, # used iff method ~ "cross-reprojection-..."
                                    method,
                                    atinfinity):
    r'''Helper for projection_uncertainty()

    This is used for the older uncertainty methods, and NOT used for
    cross-reprojection-ccp

    See docs for _propagate_calibration_uncertainty() and
    projection_uncertainty()

    This function does all the work when observing points with a finite range

    The underlying math is documented here:

    - https://mrcal.secretsauce.net/docs-3.0/uncertainty-mean-pcam.html
    - https://mrcal.secretsauce.net/docs-3.0/uncertainty-cross-reprojection.html

    The end result for method == "mean-pcam":

      q* - q

      ~   dq_dpcam (pcam* - pcam)
        + dq_dintrinsics db[intrinsics_this]

      ~   dq_dpcam/Ncam_frame sum(dpcam__drt_camj_ref db[extrinsics_j] +
                                  dpcam__dpref_i (pref_i* - pref_i) )
        + dq_dintrinsics db[intrinsics_this]

      ~   dq_dpcam/Ncam_frame sum(dpcam__drt_camj_ref db[extrinsics_j] +
                                  dpcam__dpref_i ( dpref__drt_ref_framei db[frame_i] +
                                                   dpref__dpframe_i d(pframe_i) )
        + dq_dintrinsics db[intrinsics_this]

      Here I'm assuming fixed pframe, so d(pframe_) = 0:

      dq
      ~   dq_dpcam/Ncam_frame sum(dpcam__drt_camj_ref db[extrinsics_j] +
                                  dpcam__dpref_i dpref__drt_ref_framei db[frame_i] )
        + dq_dintrinsics db[intrinsics_this]

      --->

      dq/db[extrinsics_j]    = dq_dpcam/Ncam_frame sum(dpcam__drt_camj_ref)
      dq/db[frames_i]        = dq_dpcam/Ncam_frame sum(dpcam__dpref_i dpref__drt_ref_framei )
      dq/db[intrinsics_this] = dq_dintrinsics

    '''

    # rt_cam_ref and rt_ref_frame contain poses. These are
    # available here, whether they're being optimized or not. istate_... are the
    # state variables. These may be None if the quantity in question is fixed.

    if rt_cam_ref   is None: rt_cam_ref   = np.zeros((0,6), dtype=float)
    if rt_ref_frame is None: rt_ref_frame = np.zeros((0,6), dtype=float)

    # shape (Ncameras_extrinsics,6) where Ncameras_extrinsics may be 1
    rt_cam_ref   = nps.atleast_dims(rt_cam_ref,   -2)
    # shape (Nframes,6) where Nframes may be 1
    rt_ref_frame = nps.atleast_dims(rt_ref_frame, -2)

    # These refer to the geometry, NOT the optimization. Fixed frames or cameras
    # or points ARE included here
    Ncameras_extrinsics = rt_cam_ref.shape[0]
    Nframes             = rt_ref_frame.shape[0]

    ### The output array. This function fills this in, and returns it
    # shape (..., 2,Nstate)
    dq_db = np.zeros(p_cam.shape[:-1] + (2,Nstate), dtype=float)

    # shape (..., Ncameras_extrinsics, 3)
    if not atinfinity:
        p_ref = \
            mrcal.transform_point_rt( rt_cam_ref,
                                      nps.dummy(p_cam,-2),
                                      inverted = True)
    else:
        p_ref = \
            mrcal.rotate_point_r( -rt_cam_ref[...,:3],
                                  nps.dummy(p_cam,-2) )

    # shape (..., 2,3)
    # shape (..., 2,Nintrinsics)
    _, dq_dpcam, dq_dintrinsics = \
        mrcal.project( p_cam, lensmodel, intrinsics_data,
                       get_gradients = True)

    if slice_intrinsics_state is not None:
        dq_db[         ...,slice_intrinsics_state] = \
        dq_dintrinsics[...,slice_intrinsics_arg]

    if not atinfinity:
        # shape (..., Ncameras_extrinsics,3,6)
        # shape (..., Ncameras_extrinsics,3,3)
        _, dpcam_drt_cam_ref, dpcam_dpref = \
            mrcal.transform_point_rt(rt_cam_ref, p_ref,
                                     get_gradients = True)
    else:
        # shape (..., Ncameras_extrinsics,3,3)
        # shape (..., Ncameras_extrinsics,3,3)
        _, dpcam_dr_cam_ref, dpcam_dpref = \
            mrcal.rotate_point_r(rt_cam_ref[...,:3], p_ref,
                                 get_gradients = True)

    if slice_extrinsics_state is not None:
        # I want dq/db[extrinsics_j] = dq_dpcam/Ncam_frame sum(dpcam__drt_camj_ref)
        #
        # Here I'm computing the mean over all camera,frame combinations. The
        # quantity is constant for each frame, so I use the mean over the
        # extrinsics. The gradients are distributed across the state vector, but
        # the mean comes through as the /Ncameras_extrinsics

        # shape (..., 2, Ncameras_extrinsics,6)
        dq_db_slice_extrinsics = \
            dq_db[..., slice_extrinsics_state]. \
                  reshape(dq_db.shape[:-1] + (Ncameras_extrinsics,6) )

        if not atinfinity:
            # shape (..., 2, Ncameras_extrinsics,6)
            dq_db_slice_extrinsics[...] = \
                nps.xchg( nps.matmult(# shape (..., Ncameras_extrinsics=1,2,3)
                                      nps.dummy(dq_dpcam,-3),
                                      # shape (..., Ncameras_extrinsics,  3,6)
                                      dpcam_drt_cam_ref),
                          -2, -3 ) / Ncameras_extrinsics
        else:
            # shape (..., 2, Ncameras_extrinsics,3)
            dq_db_slice_extrinsics[...,:3] = \
                nps.xchg( nps.matmult(# shape (..., Ncameras_extrinsics=1,2,3)
                                      nps.dummy(dq_dpcam,-3),
                                      # shape (..., Ncameras_extrinsics,  3,3)
                                      dpcam_dr_cam_ref),
                          -2, -3 ) / Ncameras_extrinsics

    if method == 'mean-pcam':
        if istate_frames0 is not None:

            # shape (..., Ncameras_extrinsics, 2, 3)
            dq_dpref = \
                nps.matmult(# shape (...,          Ncameras_extrinsics=1, 2, 3)
                            nps.dummy(dq_dpcam,    -3),
                            # shape (...,          Ncameras_extrinsics,   3, 3)
                            dpcam_dpref)

            if not atinfinity:
                # shape (Nframes,Ncameras_extrinsics,3)
                p_frames = mrcal.transform_point_rt( # shape (Nframes,Ncameras_extrinsics=1,6)
                                                     nps.dummy(rt_ref_frame, -2),
                                                     # shape (..., Nframes=1, Ncameras_extrinsics, 3)
                                                     nps.dummy(p_ref,-3),
                                                     inverted = True)

                # shape (...,Nframes,Ncameras_extrinsics,3,6)
                _, \
                dpref_dframes, \
                _ = mrcal.transform_point_rt( # shape (Nframes,Ncameras_extrinsics=1,6)
                                              nps.dummy(rt_ref_frame, -2),
                                              p_frames,
                                              get_gradients = True)

            else:

                # shape (Nframes,Ncameras_extrinsics,3)
                p_frames = mrcal.rotate_point_r( # shape (Nframes,Ncameras_extrinsics=1,6)
                                                 nps.dummy(-rt_ref_frame[...,:3], -2),
                                                 # shape (..., Nframes=1, Ncameras_extrinsics, 3)
                                                 nps.dummy(p_ref,-3) )

                # shape (...,Nframes,Ncameras_extrinsics,3,6)
                _, \
                dpref_dframes, \
                _ = mrcal.rotate_point_r( # shape (Nframes,Ncameras_extrinsics=1,3)
                                          nps.dummy(rt_ref_frame[...,:3], -2),
                                          p_frames,
                                          get_gradients = True)

            # shape is either of
            #    (..., Nframes, Ncameras_extrinsics, 2, 6)
            #    (..., Nframes, Ncameras_extrinsics, 2, 3)
            # depending on atinfinity
            dq_dframes = \
                nps.matmult(# shape (...,          Nframes=1, Ncameras_extrinsics, 2, 3)
                            nps.dummy(dq_dpref,    -4),
                            # shape (...,          Nframes,   Ncameras_extrinsics, 3, 6)
                            dpref_dframes)

            # shape (..., 2, Nframes,6)
            dq_db_slice_frames = \
                dq_db[...,
                      istate_frames0:
                      istate_frames0 + Nframes*6]. \
                      reshape(dq_db.shape[:-1] + (Nframes,6) )
            if not atinfinity:
                # shape (..., 2, Nframes,6)
                dq_db_slice_frames[...] = \
                    nps.xchg( np.mean(dq_dframes, axis=-3),
                              -2, -3 ) / Nframes
            else:
                # shape (..., 2, Nframes,3)
                dq_db_slice_frames[...,:3] = \
                    nps.xchg( np.mean(dq_dframes, axis=-3),
                              -2, -3 ) / Nframes

    elif method == 'cross-reprojection-rrp-Jfp':
        _dq_db__Kunpacked_rrp(## write output here
                              # shape (..., 2,Nstate)
                              dq_db,
                              ## inputs
                              # shape (..., Ncameras_extrinsics, 3)
                              p_ref,
                              # shape (..., 2,3)
                              dq_dpcam,
                              # shape (..., Ncameras_extrinsics,3,3)
                              dpcam_dpref,
                              # shape (6,Nstate)
                              Kunpacked_cross_reprojection,
                              atinfinity = atinfinity)
    else:
        raise Exception(f"Unknown {method=}")


    return dq_db



def projection_uncertainty( p_cam, model,
                            *,
                            method     = 'cross-reprojection-ccp',
                            atinfinity = False,

                            # what we're reporting
                            what = 'covariance',
                            observed_pixel_uncertainty = None):
    r'''Compute the projection uncertainty of a camera-referenced point

This is the interface to the uncertainty computations described in
https://mrcal.secretsauce.net/uncertainty.html

SYNOPSIS

    model = mrcal.cameramodel("xxx.cameramodel")

    q        = np.array((123., 443.))
    distance = 10.0

    pcam = distance * mrcal.unproject(q, *model.intrinsics(), normalize=True)

    print(mrcal.projection_uncertainty(pcam,
                                       model = model,
                                       what  = 'worstdirection-stdev'))
    ===> 0.5

    # So if we have observed a world point at pixel coordinates q, and we know
    # it's 10m out, then we know that the standard deviation of the noise of the
    # pixel observation is 0.5 pixels, in the worst direction

After a camera model is computed via a calibration process, the model is
ultimately used in projection/unprojection operations to map between 3D
camera-referenced coordinates and projected pixel coordinates. We never know the
parameters of the model perfectly, and it is VERY useful to have the resulting
uncertainty of projection. This can be used, among other things, to

- propagate the projection noise down to whatever is using the observed pixels
  to do stuff

- evaluate the quality of calibrations, to know whether a given calibration
  should be accepted, or rejected

- evaluate the stability of a computed model

I quantify uncertainty by propagating the expected pixel observation noise
through the optimization problem. This gives us the uncertainty of the solved
optimization parameters. And then we propagate this parameter noise through
projection to produce the projected pixel uncertainty.

As noted in the docs (https://mrcal.secretsauce.net/uncertainty.html), this
measures the SAMPLING error, which is a direct function of the quality of the
gathered calibration data. It does NOT measure model errors, which arise from
inappropriate lens models, for instance.

The uncertainties can be visualized with the mrcal-show-projection-uncertainty
tool.

ARGUMENTS

This function accepts an array of camera-referenced points p_cam, a model and a
few meta-parameters that describe details of the behavior. This function
broadcasts on p_cam only. We accept

- p_cam: a numpy array of shape (..., 3). This is the set of camera-coordinate
  points where we're querying uncertainty. if not atinfinity: then the full 3D
  coordinates of p_cam are significant, even distance to the camera. if
  atinfinity: the distance to the camera is ignored.

- model: a mrcal.cameramodel object that contains optimization_inputs, which are
  used to propagate the uncertainty

- method: optional string, selecting the uncertainty-computing method. Must be
  one of

  - 'cross-reprojection-ccp'
  - 'mean-pcam'
  - 'cross-reprojection-rrp-Jfp'

  The default is 'cross-reprojection-ccp'. 'mean-pcam' is the method used in
  mrcal < 3.0, and 'cross-reprojection--rrp-Jfp' is experimental. Most people
  should not touch this.

- atinfinity: optional boolean, defaults to False. If True, we want to know the
  projection uncertainty, looking at a point infinitely-far away. We propagate
  all the uncertainties, ignoring the translation components of the poses

- what: optional string, defaults to 'covariance'. This chooses what kind of
  output we want. Known options are:

  - 'covariance':           return a full (2,2) covariance matrix Var(q) for
                            each p_cam
  - 'worstdirection-stdev': return the worst-direction standard deviation for
                            each p_cam

  - 'rms-stdev':            return the RMS of the standard deviations in each
                            direction

- observed_pixel_uncertainty: optional value, defaulting to None. The
  uncertainty of the pixel observations being propagated through the solve and
  through projection. If omitted or None, this input uncertainty is inferred
  from the residuals at the optimum. Most people should omit this

RETURN VALUE

A numpy array of uncertainties. If p_cam has shape (..., 3) then:

if what == 'covariance': we return an array of shape (..., 2,2)
else:                    we return an array of shape (...)

    '''


    known_methods = set(('mean-pcam',
                         'cross-reprojection-rrp-Jfp',
                         'cross-reprojection-ccp'),)
    if method not in known_methods:
        raise Exception(f"Unknown uncertainty method: '{method}'. I know about {known_methods}")


    # which calibration-time camera we're looking at
    icam_intrinsics = model.icam_intrinsics()


    # The intrinsics,extrinsics,frames,points MUST come from the solve when
    # evaluating the uncertainties. The user is allowed to update the extrinsics
    # in the model after the solve, as long as I use the solve-time ones for the
    # uncertainty computation. Updating the intrinsics invalidates the
    # uncertainty stuff so I COULD grab those from the model. But for good
    # hygiene I get them from the solve as well
    optimization_inputs = model.optimization_inputs()
    if optimization_inputs is None:
        raise Exception("optimization_inputs are unavailable in this model. Uncertainty cannot be computed")
    lensmodel       = optimization_inputs['lensmodel']
    intrinsics_data = optimization_inputs['intrinsics'][icam_intrinsics]

    istate_intrinsics0        = mrcal.state_index_intrinsics(icam_intrinsics, **optimization_inputs)
    Nstates_intrinsics        = mrcal.num_intrinsics_optimization_params(**optimization_inputs)
    if istate_intrinsics0 is not None:
        slice_intrinsics_state = slice(istate_intrinsics0,
                                       istate_intrinsics0 + Nstates_intrinsics)

        if not optimization_inputs.get('do_optimize_intrinsics_core',True) and \
           mrcal.lensmodel_metadata_and_config(lensmodel)['has_core']:

            slice_intrinsics_arg = slice(4, 4+Nstates_intrinsics)
        else:
            slice_intrinsics_arg = slice(0, 0+Nstates_intrinsics)

    else:
        slice_intrinsics_state = None

    Nstate = mrcal.num_states(**optimization_inputs)


    if method == 'cross-reprojection-ccp':

        # q*    = project( pcam*, intrinsics* )
        # pcam* = Tc*c pcam
        #
        # So q* depends on Tc*c, intrinsics*:
        #   dq*/db =
        #     dq*/dintrinsics* dintrinsics*/db +
        #     dq*/dpcam* dpcam*/drt_cam_camp drt_cam_camp/db
        #
        # dq*/dintrinsics* ~ dq/dintrinsics: it comes directly from the
        # projection
        #
        # dintrinsics*/db ~ dintrinsics/db ~ I. The intrinsics are stored
        # directly in the state vector b, modulo some scaling
        #
        # dq*/dpcam* ~ dq/dpcam: ALSO comes directly from the projection
        #
        # dpcam*/drt_cam_camp we derive a few lines down
        #
        # drt_cam_camp/db is Kunpacked_cross_reprojection

        # pcam = transform(rt_cam_cam*, pcam*)
        #   where rt_cam_cam* is tiny
        # -> pcam ~ pcam* + cross(r_cam_cam*, pcam) + t_cam_cam*
        #         = pcam* - cross(pcam, r_cam_cam*) + t_cam_cam*
        # -> 0 = dpcam*/dr_cam_cam* - skew(pcam)
        #    0 = dpcam*/dt_cam_cam* + I
        # -> dpcam*/dr_cam_cam* = skew(pcam)
        #    dpcam*/dt_cam_cam* = -I

        Kunpacked_cross_reprojection = \
            mrcal.drt_cross_reprojection__dbpacked(icam_intrinsics = icam_intrinsics,
                                                   **optimization_inputs)
        # The value was packed in the denominator. So I call pack() to unpack it
        mrcal.pack_state(Kunpacked_cross_reprojection, **optimization_inputs)


        ### The output array. This function fills this in, and returns it
        # shape (..., 2,Nstate)
        dq_db = np.zeros(p_cam.shape[:-1] + (2,Nstate), dtype=float)

        # shape (..., 2,3)
        # shape (..., 2,Nintrinsics)
        _, dq_dpcam, dq_dintrinsics = \
            mrcal.project( p_cam, lensmodel, intrinsics_data,
                           get_gradients = True)

        if slice_intrinsics_state is not None:
            dq_db[         ...,slice_intrinsics_state] = \
            dq_dintrinsics[...,slice_intrinsics_arg]

        dpcamp__dr_cam_camp = mrcal.skew_symmetric(p_cam)

        # I don't explicitly store dpcam*/dt = -I. I do that implicitly

        dq__dr_cam_camp = nps.matmult(dq_dpcam, dpcamp__dr_cam_camp)

        # I apply this to the whole dq_db array. Kunpacked_cross_reprojection has 0 rows for
        # the unaffected state, so the columns that should be untouched will
        # be untouched
        dq_db += nps.matmult(dq__dr_cam_camp, Kunpacked_cross_reprojection[:3,:])

        if not atinfinity:
            dq_db -= nps.matmult(dq_dpcam, Kunpacked_cross_reprojection[3:,:])

        return \
            _propagate_calibration_uncertainty(what,
                                               dF_dbunpacked              = dq_db,
                                               observed_pixel_uncertainty = observed_pixel_uncertainty,
                                               optimization_inputs        = optimization_inputs)


    # The math implemented here is documented in
    #
    #   https://mrcal.secretsauce.net/uncertainty.html

    # Non-None if this:
    # - exists
    # - isn't None
    # - has non-zero elements
    def get_input(what):
        x = optimization_inputs.get(what)
        if x is not None and x.size > 0: return x
        else:                            return None


    # These may or may not be optimized. The istate_... variables are None if
    # the particular quantity isn't up for optimization
    rt_ref_frame = get_input('rt_ref_frame')
    istate_frames0  = mrcal.state_index_frames(0, **optimization_inputs)

    if re.match('cross-reprojection', method):
        Kunpacked_cross_reprojection = \
            mrcal.drt_cross_reprojection__dbpacked(icam_intrinsics = -1,
                                                   **optimization_inputs)
        # The value was packed in the denominator. So I call pack() to unpack it
        mrcal.pack_state(Kunpacked_cross_reprojection, **optimization_inputs)
    else:
        Kunpacked_cross_reprojection = None
        if optimization_inputs.get('do_optimize_frames'):
            if get_input('observations_point')              is not None or \
               get_input('observations_point_triangulated') is not None:
                raise Exception(f"We have point observations that we are optimizing; only cross-reprojection uncertainty can work here. Have {method=}")
            if rt_ref_frame is None and points is None:
                raise Exception("Some frames or points must exist for the no-cross-reprojection uncertainty computation, but we don't have any")


    # Now the extrinsics. I look at all the ones that correspond with the
    # specific camera I care about. If the camera is stationary, this will
    # produce exactly one set of extrinsics. If the camera is moving, we may get
    # more than one. At this time I limit to myself to a consecutive block of
    # extrinsics vectors. Once this all works I can relax that requirement
    ifcice = optimization_inputs['indices_frame_camintrinsics_camextrinsics']
    if ifcice is None: ifcice = np.zeros((0,3),dtype=np.int32)
    ipcice = optimization_inputs['indices_point_camintrinsics_camextrinsics']
    if ipcice is None: ipcice = np.zeros((0,3),dtype=np.int32)
    icice = nps.glue(ifcice[:,1:], ipcice[:,1:], axis=-2)

    icam_extrinsics = np.unique( icice[icice[:,0] == icam_intrinsics, 1] ) # sorted
    if icam_extrinsics.size == 0:
        raise Exception(f"No extrinsics corresponding to {icam_intrinsics=}. I don't know what to do")
    if icam_extrinsics.size > 1:
        d = np.unique(np.diff(icam_extrinsics))
        if not (d.size == 1 and d[0] == 1):
            raise Exception("At this point I'm only supporting consecutive block of extrinsics for a given icam_intrinsics")
    if icam_extrinsics[0] < 0:
        if icam_extrinsics.size == 1:
            # Stationary camera, at the reference
            rt_cam_ref = mrcal.identity_rt()
            slice_extrinsics_state = None


        else:
            # Moving camera. One of the poses is at the reference. This requires
            # more typing. I'll do this later
            raise Exception("Have moving camera, some poses are at the reference. This isn't supported yet")

    else:
        # I'm guaranteed to get rt_cam_ref with the right number of extrinsics
        # (all the ones that correspond to this icam_intrinsics). And I know
        # they're a contiguous block in my optimization vector starting with
        # istate_extrinsics0
        rt_cam_ref = get_input('rt_cam_ref')[icam_extrinsics,:]
        istate_extrinsics0 = mrcal.state_index_extrinsics(icam_extrinsics[0],
                                                          **optimization_inputs)
        Ncameras_extrinsics = rt_cam_ref.shape[0]
        slice_extrinsics_state = slice(istate_extrinsics0,
                                       istate_extrinsics0 + Ncameras_extrinsics*6)


    # shape (..., 2, Nstate)
    dq_db = \
        _dq_db__projection_uncertainty( p_cam,
                                        lensmodel, intrinsics_data,
                                        rt_cam_ref,
                                        rt_ref_frame,
                                        Nstate,
                                        slice_intrinsics_state,
                                        slice_intrinsics_arg,
                                        slice_extrinsics_state,
                                        istate_frames0,
                                        atinfinity = atinfinity,
                                        method     = method,
                                        Kunpacked_cross_reprojection = Kunpacked_cross_reprojection)

    return \
        _propagate_calibration_uncertainty(what,
                                           dF_dbunpacked              = dq_db,
                                           observed_pixel_uncertainty = observed_pixel_uncertainty,
                                           optimization_inputs        = optimization_inputs)


def projection_diff(models,
                    *,
                    implied_Rt10 = None,
                    gridn_width  = 60,
                    gridn_height = None,

                    intrinsics_only = False,
                    distance        = None,

                    use_uncertainties     = True,
                    focus_center          = None,
                    focus_radius          = -1.):
    r'''Compute the difference in projection between N models

SYNOPSIS

    models = ( mrcal.cameramodel('dance0-cam0.cameramodel'),
               mrcal.cameramodel('dance1-cam0.cameramodel') )

    difference,_,q0,_ = mrcal.projection_diff(models)

    print(q0.shape)
    ==> (40,60,2)

    print(difference.shape)
    ==> (40,60)

    # The differences are computed across a grid. 'q0' is the pixel centers of
    # each grid cell. 'difference' is the projection variation between the two
    # models at each cell

The operation of this function is documented at
https://mrcal.secretsauce.net/differencing.html

It is often useful to compare the projection behavior of two camera models. For
instance, one may want to validate a calibration by comparing the results of two
different chessboard dances. Or one may want to evaluate the stability of the
intrinsics in response to mechanical or thermal stresses. This function makes
these comparisons, and returns the results. A visualization wrapper is
available: mrcal.show_projection_diff() and the mrcal-show-projection-diff tool.

In the most common case we're given exactly 2 models to compare, and we compute
the differences in projection of each point. If we're given more than 2 models,
we instead compute the standard deviation of the differences between models 1..N
and model0.

The top-level operation of this function:

- Grid the imager
- Unproject each point in the grid using one camera model
- Apply a transformation to map this point from one camera's coord system to the
  other. How we obtain this transformation is described below
- Project the transformed points to the other camera
- Look at the resulting pixel difference in the reprojection

If implied_Rt10 is given, we simply use that as the transformation (this is
currently supported ONLY for diffing exactly 2 cameras). If implied_Rt10 is not
given, we estimate it. Several variables control this. Top-level logic:

  if intrinsics_only:
      Rt10 = identity_Rt()
  else:
      if focus_radius == 0:
          Rt10 = relative_extrinsics(models)
      else:
          Rt10 = implied_Rt10__from_unprojections()

Sometimes we want to look at the intrinsics differences in isolation (if
intrinsics_only), and sometimes we want to use the known geometry in the given
models (not intrinsics_only and focus_radius == 0). If neither of these apply,
we estimate the transformation: this is needed if we're comparing different
calibration results from the same lens.

Given different camera models, we have a different set of intrinsics for each.
Extrinsics differ also, even if we're looking at different calibration of the
same stationary lens: the position and orientation of the camera coordinate
system in respect to the physical camera housing shift with each calibration.
This geometric variation is baked into the intrinsics. So when we project "the
same world point" into both cameras (as is desired when comparing repeated
calibrations), we must apply a geometric transformation because we want to be
comparing projections of world points (relative to the camera housing), not
projections relative to the (floating) camera coordinate systems. This
transformation is unknown, but we can estimate it by fitting projections across
the imager: the "right" transformation would result in apparent low projection
differences in a wide area.

This transformation is computed by implied_Rt10__from_unprojections(), and some
details of its operation are significant:

- The imager area we use for the fit
- Which world points we're looking at

In most practical usages, we would not expect a good fit everywhere in the
imager: areas where no chessboards were observed will not fit well, for
instance. From the point of view of the fit we perform, those ill-fitting areas
should be treated as outliers, and they should NOT be a part of the solve. How
do we specify the well-fitting area? The best way is to use the model
uncertainties: these can be used to emphasize the confident regions of the
imager. This behavior is selected with use_uncertainties=True, which is the
default. If uncertainties aren't available, or if we want a faster solve, pass
use_uncertainties=False. The well-fitting region can then be passed using the
focus_center,focus_radius arguments to indicate the circle in the imager we care
about.

If use_uncertainties then the defaults for focus_center,focus_radius are set to
utilize all the data in the imager. If not use_uncertainties, then the defaults
are to use a more reasonable circle of radius min(width,height)/6 at the center
of the imager. Usually this is sufficiently correct, and we don't need to mess
with it. If we aren't guided to the correct focus region, the
implied-by-the-intrinsics solve will try to fit lots of outliers, which would
result in an incorrect transformation, which in turn would produce overly-high
reported diffs. A common case when this happens is if the chessboard
observations used in the calibration were concentrated to the side of the image
(off-center), no uncertainties were used, and the focus_center was not pointed
to that area.

Unlike the projection operation, the diff operation is NOT invariant under
geometric scaling: if we look at the projection difference for two points at
different locations along a single observation ray, there will be a variation in
the observed diff. This is due to the geometric difference in the two cameras.
If the models differed only in their intrinsics parameters, then this variation
would not appear. Thus we need to know how far from the camera to look, and this
is specified by the "distance" argument. By default (distance = None) we look
out to infinity. If we care about the projection difference at some other
distance, pass that here. Multiple distances can be passed in an iterable. We'll
then fit the implied-by-the-intrinsics transformation using all the distances,
and we'll return the differences for each given distance. Generally the most
confident distance will be where the chessboards were observed at calibration
time.

ARGUMENTS

- models: iterable of mrcal.cameramodel objects we're comparing. Usually there
  will be 2 of these, but more than 2 is allowed. The intrinsics are always
  used; the extrinsics are used only if not intrinsics_only and focus_radius==0

- implied_Rt10: optional transformation to use to line up the camera coordinate
  systems. Most of the time we want to estimate this transformation, so this
  should be omitted or None. Currently this is supported only if exactly two
  models are being compared.

- gridn_width: optional value, defaulting to 60. How many points along the
  horizontal gridding dimension

- gridn_height: how many points along the vertical gridding dimension. If None,
  we compute an integer gridn_height to maintain a square-ish grid:
  gridn_height/gridn_width ~ imager_height/imager_width

- intrinsics_only: optional boolean, defaulting to False. If True: we evaluate
  the intrinsics of each lens in isolation by assuming that the coordinate
  systems of each camera line up exactly

- distance: optional value, defaulting to None. Has an effect only if not
  intrinsics_only. The projection difference varies depending on the range to
  the observed world points, with the queried range set in this 'distance'
  argument. If None (the default) we look out to infinity. We can compute the
  implied-by-the-intrinsics transformation off multiple distances if they're
  given here as an iterable. This is especially useful if we have uncertainties,
  since then we'll emphasize the best-fitting distances.

- use_uncertainties: optional boolean, defaulting to True. Used only if not
  intrinsics_only and focus_radius!=0. If True we use the whole imager to fit
  the implied-by-the-intrinsics transformation, using the uncertainties to
  emphasize the confident regions. If False, it is important to select the
  confident region using the focus_center and focus_radius arguments. If
  use_uncertainties is True, but that data isn't available, we report a warning,
  and try to proceed without.

- focus_center: optional array of shape (2,); the imager center by default. Used
  only if not intrinsics_only and focus_radius!=0. Used to indicate that the
  implied-by-the-intrinsics transformation should use only those pixels a
  distance focus_radius from focus_center. This is intended to be used if no
  uncertainties are available, and we need to manually select the focus region.

- focus_radius: optional value. If use_uncertainties then the default is LARGE,
  to use the whole imager. Else the default is min(width,height)/6. Used to
  indicate that the implied-by-the-intrinsics transformation should use only
  those pixels a distance focus_radius from focus_center. This is intended to be
  used if no uncertainties are available, and we need to manually select the
  focus region. To avoid computing the transformation, either pass
  focus_radius=0 (to use the extrinsics in the given models) or pass
  intrinsics_only=True (to use the identity transform).

RETURNED VALUE

A tuple

- difflen: a numpy array of shape (...,gridn_height,gridn_width) containing the
  magnitude of differences at each cell, or the standard deviation of the
  differences between models 1..N and model0 if len(models)>2. if
  len(models)==2: this is nps.mag(diff). If the given 'distance' argument was an
  iterable, the shape is (len(distance),...). Otherwise the shape is (...)

- diff: a numpy array of shape (...,gridn_height,gridn_width,2) containing the
  vector of differences at each cell. If len(models)>2 this isn't defined, so
  None is returned. If the given 'distance' argument was an iterable, the shape
  is (len(distance),...). Otherwise the shape is (...)

- q0: a numpy array of shape (gridn_height,gridn_width,2) containing the
  pixel coordinates of each grid cell

- Rt10: the geometric Rt transformation in an array of shape (...,4,3). This is
  the relative transformation we ended up using, which is computed using the
  logic above (using intrinsics_only and focus_radius). if len(models)>2: this
  is an array of shape (len(models)-1,4,3), with slice i representing the
  transformation between camera 0 and camera i+1.

    '''

    if len(models) < 2:
        raise Exception("At least 2 models are required to compute the diff")
    if len(models) > 2 and implied_Rt10 is not None:
        raise Exception("A given implied_Rt10 is currently supported ONLY if exactly 2 models are being compared")

    # If the distance is iterable, the shape of the output is (len(distance),
    # ...). Otherwise it is just (...). In the intermediate computations, the
    # quantities ALWAYS have shape (len(distance), ...). I select the desired
    # shape at the end
    distance_is_iterable = True
    try:    len(distance)
    except: distance_is_iterable = False

    if distance is None:
        atinfinity = True
        distance   = np.ones((1,), dtype=float)
    else:
        atinfinity = False
        distance   = nps.atleast_dims(np.array(distance), -1)
    distance   = nps.mv(distance.ravel(), -1,-4)

    imagersizes = np.array([model.imagersize() for model in models])
    if np.linalg.norm(np.std(imagersizes, axis=-2)) != 0:
        raise Exception("The diff function needs all the imager dimensions to match. Instead got {}". \
                        format(imagersizes))
    W,H=imagersizes[0]

    lensmodels      = [model.intrinsics()[0] for model in models]
    intrinsics_data = [model.intrinsics()[1] for model in models]

    for i in range(len(models)):
        if mrcal.lensmodel_metadata_and_config(lensmodels[i])['noncentral'] and not \
           (re.match("LENSMODEL_CAHVORE_",models[i].intrinsics()[0]) and nps.norm2(models[i].intrinsics()[1][-3:]) < 1e-12):
            if not atinfinity:
                raise Exception(f"Model {i} is noncentral, so I can only evaluate the diff at infinity")
            if re.match("LENSMODEL_CAHVORE_",lensmodels[i]):
                if use_uncertainties:
                    raise Exception("I have a noncentral model. No usable uncertainties for those yet")
                # Special-case to centralize CAHVORE. This path will need to be
                # redone when I do noncentral models "properly", but this will
                # do in the meantime
                intrinsics_data[i][-3:] = 0
            else:
                raise Exception("I have a non-CAHVORE noncentral model. This isn't supported yet")

    # v  shape (Ncameras,Nheight,Nwidth,3)
    # q0 shape (         Nheight,Nwidth,2)
    v,q0 = mrcal.sample_imager_unproject(gridn_width, gridn_height,
                                         W, H,
                                         lensmodels, intrinsics_data,
                                         normalize = True)

    uncertainties = None
    if use_uncertainties and \
       not intrinsics_only and focus_radius != 0 and \
       implied_Rt10 is None:
        try:
            # len(uncertainties) = Ncameras. Each has shape (len(distance),Nh,Nw)
            uncertainties = \
                [ mrcal.projection_uncertainty(# shape (len(distance),Nheight,Nwidth,3)
                                               v[i] * distance,
                                               models[i],
                                               atinfinity = atinfinity,
                                               what       = 'worstdirection-stdev') \
                  for i in range(len(models)) ]
        except Exception as e:
            print(f"WARNING: projection_diff() was asked to use uncertainties, but they aren't available/couldn't be computed. Falling back on the region-based-only logic. Caught exception: {e}",
                  file = sys.stderr)

    if focus_center is None:
        focus_center = ((W-1.)/2., (H-1.)/2.)

    if focus_radius < 0:
        if uncertainties is not None:
            focus_radius = float(max(W,H) * 100.) # whole imager
        else:
            focus_radius = float(min(W,H)/6.)

    if len(models) == 2:
        # Two models. Take the difference and call it good
        if implied_Rt10 is not None:
            Rt10 = implied_Rt10

        else:
            if intrinsics_only:
                Rt10 = mrcal.identity_Rt()
            else:
                if focus_radius == 0:
                    Rt10 = mrcal.compose_Rt(models[1].Rt_cam_ref(),
                                            models[0].Rt_ref_cam())
                else:
                    # weights has shape (len(distance),Nh,Nw))
                    if uncertainties is not None:
                        weights = 1.0 / (uncertainties[0]*uncertainties[1])

                        # It appears to work better if I discount the uncertain regions
                        # even more. This isn't a principled decision, and is supported
                        # only by a little bit of data. The differencing.org I'm writing
                        # now will contain a weighted diff of culled and not-culled
                        # splined model data. That diff computation requires this.
                        weights *= weights
                    else:
                        weights = None

                    # weight may be inf or nan. implied_Rt10__from_unprojections() will
                    # clean those up, as well as any inf/nan in v (from failed
                    # unprojections)
                    Rt10 = \
                        implied_Rt10__from_unprojections(q0,
                                                         # shape (len(distance),Nheight,Nwidth,3)
                                                         v[0,...] * distance,
                                                         v[1,...],
                                                         weights      = weights,
                                                         atinfinity   = atinfinity,
                                                         focus_center = focus_center,
                                                         focus_radius = focus_radius)


        q1 = mrcal.project( mrcal.transform_point_Rt(Rt10,
                                                     # shape (len(distance),Nheight,Nwidth,3)
                                                     v[0,...] * distance),
                            lensmodels[1], intrinsics_data[1])
        # shape (len(distance),Nheight,Nwidth,2)
        q1 = nps.atleast_dims(q1, -4)

        diff    = q1 - q0
        difflen = nps.mag(diff)

    else:

        # Many models. Look at the stdev
        def get_Rt10( i0, i1 ):

            if intrinsics_only:
                return mrcal.identity_Rt()

            if focus_radius == 0:
                return mrcal.compose_Rt(models[i1].Rt_cam_ref(),
                                        models[i0].Rt_ref_cam())

            # weights has shape (len(distance),Nh,Nw))
            if uncertainties is not None:
                weights = 1.0 / (uncertainties[i0]*uncertainties[i1])

                # It appears to work better if I discount the uncertain regions
                # even more. This isn't a principled decision, and is supported
                # only by a little bit of data. The differencing.org I'm writing
                # now will contain a weighted diff of culled and not-culled
                # splined model data. That diff computation requires this.
                weights *= weights
            else:
                weights = None

            v0 = v[i0,...]
            v1 = v[i1,...]

            return \
                implied_Rt10__from_unprojections(q0,
                                                 # shape (len(distance),Nheight,Nwidth,3)
                                                 v0*distance,
                                                 v1,
                                                 weights      = weights,
                                                 atinfinity   = atinfinity,
                                                 focus_center = focus_center,
                                                 focus_radius = focus_radius)
        def get_reprojections(q0, Rt10,
                              lensmodel, intrinsics_data):
            q1 = mrcal.project(mrcal.transform_point_Rt(Rt10,
                                                        # shape (len(distance),Nheight,Nwidth,3)
                                                        v[0,...]*distance),
                               lensmodel, intrinsics_data)
            # returning shape (len(distance),Nheight,Nwidth,2)
            return nps.atleast_dims(q1, -4)

        Ncameras = len(v)
        # shape (Ncameras-1, 4,3)
        Rt10 = nps.cat(*[ get_Rt10(0,i) for i in range(1,Ncameras)])

        # shape (Ncameras-1,len(distance),Nheight,Nwidth,2)
        grids = nps.cat(*[get_reprojections(q0, Rt10[i-1],
                                            lensmodels[i], intrinsics_data[i]) \
                          for i in range(1,Ncameras)])

        diff    = None
        # shape (len(distance),Nheight,Nwidth)
        difflen = np.sqrt(np.mean(nps.norm2(grids-q0),axis=0))

    # difflen, diff, q0 currently all have shape (len(distance), ...). If the
    # given distance was NOT an iterable, I strip out that leading dimension
    if not distance_is_iterable:
        if difflen.shape[0] != 1:
            raise Exception(f"distance_is_iterable is False, but leading shape of difflen is not 1 (difflen.shape = {difflen.shape}). This is a bug. Giving up")
        difflen = difflen[0,...]

        if diff is not None:
            if diff.shape[0] != 1:
                raise Exception(f"distance_is_iterable is False, but leading shape of diff is not 1 (diff.shape = {diff.shape}). This is a bug. Giving up")
            diff = diff[0,...]

    return difflen, diff, q0, Rt10


def stereo_pair_diff(model_pairs,
                     *,
                     gridn_width  = 60,
                     gridn_height = None,
                     distance     = None):
    r'''Compute the difference in projection between 2 pairs of models

SYNOPSIS

    model_pairs = (( "set1/camera-0.cameramodel", "set1/camera-3.cameramodel", ),
                   ( "set2/camera-0.cameramodel", "set2/camera-3.cameramodel", ))
    model_pairs = [[mrcal.cameramodel(m) for m in p] for p in model_pairs]
    difference,diff,q0 = mrcal.stereo_pair_diff(model_pairs)

    print(q0.shape)
    ==> (40,60,2)

    print(difference.shape)
    ==> (40,60)

    # The differences are computed across a grid. 'q0' is the pixel centers of
    # each grid cell on the camera0 imager.

    'difference' show the mismatch of mapping q0 from camera0 to camera1

THIS TOOL IS EXPERIMENTAL, AND LIKELY WILL BE CHANGED AND EXTENDED. MORE
COMPLETE DOCUMENTATION WILL BE WRITTEN LATER

It is useful to evaluate the stability and uncertainty of a multi-camera system,
looking at the intrinsics,extrinsics either separately or jointly. For instance,
when deployed in the field a calibration may shift in the extrinsics (camera
units moved) and/or intrinsics (lens moved), and it would be useful to quantify
them individually. Up to now (version 2.5) mrcal has been concerned mostly with
the intrinsics: the mean-pcam uncertainty method and
implied_Rt10__from_unprojections() try to compensate for and eliminate the
effect of extrinsic shifts in uncertainty and diff methods respectively.

This stereo_pair_diff() tool extends this in one way: in a multi-camera
calibration, it quantifies the joint intrinsics+extrinsics shift of a camera
pair. At a given distance this tool reprojects pixels in camera0 to camera1,
and compares the results of this reprojection. This is actually far simpler than
the earlier intrinsics-only methods: no implied transform or reference-frame
shift need to be considered. As of mrcal 2.5 we also have an extrinsics-only
stereo-pair diff in an unreleased analyses/extrinsics-stability.py tool. No
corresponding uncertainty method is implemented yet, but that is
straightforward. All the methods will be implemented in a future mrcal release.
They can then be studied to see what is useful to look at in practice.

The reason mrcal started out with the more complex intrinsics-only methods is to
support monocular calibrations: camera-camera extrinsics don't exist in this
scenario.

The top-level operation of this function:

- Grid the camera0 imager
- Unproject each of these grid points to a given distance from camera0. Each
  pair has different camera0 intrinsics.
- Apply a transformation to map these points from camera0 coords to camera1
  coords. Each pair has its own transform
- Project each point to camera1. Each pair has different camera1 intrinsics
- Look at the resulting pixel difference in the camera1 reprojection

In most practical usages, we would not expect a good fit everywhere in the
imager: areas where no chessboards were observed will not fit well, because the
solves are uncertain there.

This diff operation is NOT invariant under geometric scaling. As with the
intrinsics-only diff reported by projection_diff(), the reported diff will be
smallest at the range where the chessboards were observed.

ARGUMENTS

- model_pairs: iterable of 2 camera pairs. Each pair is an iterable ofr 2
  mrcal.cameramodel objects. Exactly two pairs are expected. The intrinsics and
  extrinsics are used. The corresponding cameras in the pairs must have the same
  imager size, but the two cameras inside each pair may differ

- gridn_width: optional value, defaulting to 60. How many points along the
  horizontal gridding dimension of the camera0 imager

- gridn_height: how many points along the vertical gridding dimension of the
  camera0 imager. If None, we compute an integer gridn_height to maintain a
  square-ish grid: gridn_height/gridn_width ~ imager_height/imager_width

- distance: optional value, defaulting to None. The distance from camera0 to
  evaluate. If None (the default) we look out to infinity.

RETURNED VALUE

A tuple

- difflen: a numpy array of shape (...,gridn_height,gridn_width) containing the
  magnitude of differences of reprojection-to-camera1 at each camera0 cell

- diff: a numpy array of shape (...,gridn_height,gridn_width,2) containing the
  vector of differences of reprojection-to-camera1 at each camera0 cell

- q0: a numpy array of shape (gridn_height,gridn_width,2) containing the
  pixel coordinates of each camera0 grid cell

- reproject_pairp_from_cami: a function taking arguments (q,pairp,cami). For the
  given inputs to this functions, maps pixels q from pair-p-camera-i to
  pair-p-camera-other.

    '''

    if len(model_pairs) != 2:
        raise Exception("Exactly 2 model_pairs are expected")


    Rt10_pairs = np.array([ mrcal.compose_Rt( m1.Rt_cam_ref(),m0.Rt_ref_cam() ) \
                            for m0,m1 in model_pairs ])

    if distance is None:
        atinfinity = True
        distance = 1.
        for Rt10 in Rt10_pairs:
            Rt10[3,:] = 0
    else:
        atinfinity = False

    for model_pair in model_pairs:
        for model in model_pair:
            if not mrcal.lensmodel_metadata_and_config(model.intrinsics()[0])['noncentral']:
                continue
            if not re.match("LENSMODEL_CAHVORE_",model.intrinsics()[0]):
                raise Exception("I have a noncentral model that I cannot centralize. unproject() isn't implemented yet in this case")

            # CAHVORE. I know how to centralize this. If we're looking at
            # infinity, then this is ok
            if np.norm2(model.intrinsics()[1][-3:]) < 1e-12 or atinfinity:
                intrinsics_data = model.intrinsics()[1]
                intrinsics_data[-3:] = 0
                model.intrinsics( intrinsics = (model.intrinsics()[0],
                                                intrinsics_data) )
                continue
            if not atinfinity:
                raise Exception("I have a noncentral model. I can centralize it, but that's only valid at infinity")



    imagersizes0 = np.array([model_pair[0].imagersize() for model_pair in model_pairs])
    if np.any(imagersizes0[1] - imagersizes0[0]):
        raise Exception("The first camera in each given pair MUST have identical imager dimensions")
    imagersizes0 = imagersizes0[0]

    imagersizes1 = np.array([model_pair[1].imagersize() for model_pair in model_pairs])
    if np.any(imagersizes1[1] - imagersizes1[0]):
        raise Exception("The second camera in each given pair must have identical imager dimensions. I technically don't NEED this, but it's extremely likely to be an unintentional mistake by the caller")



    Rt10_Rt01_pairs = (Rt10_pairs,
                       mrcal.invert_Rt(Rt10_pairs))

    reproject_pairp_from_cami = \
        lambda q,p,i: \
        mrcal.project( mrcal.transform_point_Rt(Rt10_Rt01_pairs[i][p],
                                                distance *
                                                mrcal.unproject(q,
                                                                *model_pairs[p][i].intrinsics(),
                                                                normalize = True)),
                       *model_pairs[p][1-i].intrinsics() )

    q0 = mrcal.sample_imager(gridn_width, gridn_height,
                             *imagersizes0)

    # q1 shape (Npairs, Nheight,Nwidth,2)
    q1 = [ reproject_pairp_from_cami(q0,ipair,0) for ipair in (0,1) ]

    diff    = q1[1] - q1[0]
    difflen = nps.mag(diff)
    return difflen, diff, q0, reproject_pairp_from_cami


def is_within_valid_intrinsics_region(q, model):
    r'''Which of the pixel coordinates fall within the valid-intrinsics region?

SYNOPSIS

    mask = mrcal.is_within_valid_intrinsics_region(q, model)
    q_trustworthy = q[mask]

mrcal camera models may have an estimate of the region of the imager where the
intrinsics are trustworthy (originally computed with a low-enough error and
uncertainty). When using a model, we may want to process points that fall
outside of this region differently from points that fall within this region.
This function returns a mask that indicates whether each point is within the
region or not.

If no valid-intrinsics region is defined in the model, returns None.

ARGUMENTS

- q: an array of shape (..., 2) of pixel coordinates

- model: the model we're interrogating

RETURNED VALUE

The mask that indicates whether each point is within the region

    '''

    r = model.valid_intrinsics_region()
    if r is None:
        return None

    from shapely.geometry import Polygon,Point

    r = Polygon(r)

    mask = np.zeros(q.shape[:-1], dtype=bool)
    mask_flat = mask.ravel()
    q_flat = q.reshape(q.size//2, 2)
    for i in range(q.size // 2):
        if r.contains(Point(q_flat[i])):
            mask_flat[i] = True
    return mask


def model_resolution__deg_pixel(q, model,
                                *,
                                mode = 'mean'):
    r'''Report the model resolution in deg/pixel at q

SYNOPSIS

  W,H = model.imagersize()
  q = np.ascontiguousarray( \
      nps.mv( nps.cat(*np.meshgrid( np.linspace(0, W-1, gridn_width),
                                    np.linspace(0, H-1, gridn_height) )),
              0,-1) )
  resolution__deg_pixel = mrcal.model_resolution__deg_pixel(q, model)

This is the core operation of the mrcal-show-model-resolution tool.

For any vector v and associated pixel q, we want to gauge the shifts in q that
result from small shifts in v, in the space normal to v.

We have v = unproject(q). Let a rotation Rt = [u0 u1 v] so R v = [..., 1]. I'm
interested in motion in the space perpendicular to v: in the subspace spanned by
[u0 u1]. Let dv = [u0 u1] ab. We have

  dq = dqdv dv
     = dqdv Rt R dv
     = dqdv Rt R [u0 u1] ab
     = dqdv Rt ab0
     = dqdv [u0 v0] ab

So dq = M ab where M = dqdv [u0 u1]. M is (2,2). dq is a shift in pixel space,
ab is a shift in an orthonormal 2D plane perpendicular to the observation vector
v.

  norm2(dq) = abt MtM ab

So the smallest/largest sensitivities of dq are given by sqrt(eigenvalue(MtM))

This function supports broadcasting fully

ARGUMENTS

- q: an array of shape (..., 2) of pixel coordinates

- model: the model we're evaluating. Used for intrinsics only.

- mode: optional string, defaulting to 'mean'. Selects the eigenvalue we're
  interested in. 'min','max' report the min- and- max- resolutions respectively.
  'mean' reports the average. 'eccentricity' reports max/min

RETURNED VALUE

The scalar resolution in degrees/pixel

    '''

    if not (mode == 'min' or mode == 'max' or mode == 'mean' or mode == 'eccentricity'):
        raise Exception("Mode must be one of ('min','max','mean','eccentricity')")

    v = mrcal.unproject(q, *model.intrinsics(),
                        normalize = True)
    _,dq_dv,_ = mrcal.project(v, *model.intrinsics(),
                              get_gradients = True)

    # shape (...,3,3)
    R = mrcal.R_aligned_to_vector(v)

    # shape (...,2,2)
    M = nps.matmult(dq_dv, nps.transpose(R)[...,:2])

    # Let MtM = (a b). If l is an eigenvalue then
    #           (b c)
    #
    #     (a-l)*(c-l) - b^2 = 0 --> l^2 - (a+c) l + ac-b^2 = 0
    #
    #     --> l = (a+c +- sqrt( a^2 + 2ac + c^2 - 4ac + 4b^2)) / 2 =
    #           = (a+c +- sqrt( a^2 - 2ac + c^2 + 4b^2)) / 2 =
    #           = (a+c)/2 +- sqrt( (a-c)^2/4 + b^2)
    a = nps.inner(M[...,:,0], M[...,:,0])
    b = nps.inner(M[...,:,0], M[...,:,1])
    c = nps.inner(M[...,:,1], M[...,:,1])
    sqrt_discriminant = np.sqrt( (a-c)*(a-c)/4 + b*b)
    lmax = (a+c)/2 + sqrt_discriminant
    lmin = (a+c)/2 - sqrt_discriminant
    # handle roundoff error
    lmin = np.clip(lmin, a_min=1e-6, a_max=None)

    # swap min/max since I took the reciprocal
    resolution_max_deg_pix = 1./np.sqrt(lmin) * 180./np.pi
    resolution_min_deg_pix = 1./np.sqrt(lmax) * 180./np.pi

    if mode == 'min':          return resolution_min_deg_pix
    if mode == 'max':          return resolution_max_deg_pix
    if mode == 'eccentricity': return resolution_max_deg_pix/resolution_min_deg_pix

    return (resolution_min_deg_pix + resolution_max_deg_pix)/2.


def _report_optimization_conditioning(optimization_inputs,
                                      **kwargs):

    r'''Report some diagnostics that could cause solver difficulties

I look for directions in state space that have little effect on the
measurements. Ideally all motions in state space have a large effect.

norm2(dx) = dbt JtJ db.

So we look at eigenvectors of JtJ corresponding to smallest eigenvalues

    '''

    import scipy.sparse.linalg
    import gnuplotlib as gp

    b,x,J,F = mrcal.optimizer_callback(**optimization_inputs)


    JtJ = J.T @ J
    l_largest,v_largest = scipy.sparse.linalg.eigsh(JtJ, k=10, which='LM')

    Nstate = len(b)
    JtJ_inv_op = \
        scipy.sparse.linalg.LinearOperator((Nstate, Nstate),
                                           matvec = lambda b: F.solve_xt_JtJ_bt(b.astype(float)))

    # eig_largest(JtJ_inv) = eig_smallest(JtJ)
    l_smallest,v_smallest = scipy.sparse.linalg.eigsh(JtJ_inv_op, k=10, which='LM')
    l_smallest = 1.0/l_smallest

    gp.add_plot_option(kwargs,
                       title     = "Eigenvectors(JtJ) for the smallest eigenvalues. Quantifies norm2(dx)",
                       overwrite = True)
    gp.plot( v_smallest.T,
             legend = l_smallest,
             _set = mrcal.plotoptions_state_boundaries(**optimization_inputs),
             **kwargs)