#!/usr/bin/python3

r'''Calibrate some synchronized, stationary cameras

SYNOPSIS

  $ mrcal-calibrate-cameras
      --corners-cache corners.vnl
      --lensmodel LENSMODEL_OPENCV8
      --focal 1700 --object-spacing 0.01 --object-width-n 10
      --outdir /tmp
      --pairs
      'left*.png' 'right*.png'

    ... lots of output as the solve runs ...
    Done!
    RMS reprojection error: 1.9 pixels
    Worst reprojection error: 7.8 pixels
    Noutliers: 319 out of 17100 total points: 1.9% of the data

    Wrote /tmp/camera0-0.cameramodel
    Wrote /tmp/camera0-1.cameramodel

This tool uses the generic mrcal platform to solve a common specific problem of
N-camera calibration using observations of a chessboard. Please see the mrcal
documentation at http://mrcal.secretsauce.net/how-to-calibrate.html for details.

'''

import sys
import argparse
import re
import os

def parse_args():

    def positive_float(string):
        try:
            value = float(string)
        except:
            raise argparse.ArgumentTypeError(f"argument MUST be a positive floating-point number. Got '{string}'")
        if value <= 0:
            raise argparse.ArgumentTypeError(f"argument MUST be a positive floating-point number. Got '{string}'")
        return value
    def positive_int(string):
        try:
            value = int(string)
        except:
            raise argparse.ArgumentTypeError(f"argument MUST be a positive integer. Got '{string}'")
        if value <= 0 or abs(value-float(string)) > 1e-6:
            raise argparse.ArgumentTypeError(f"argument MUST be a positive integer. Got '{string}'")
        return value


    parser = \
        argparse.ArgumentParser(description = __doc__,
                                formatter_class=argparse.RawDescriptionHelpFormatter)
    parser.add_argument('--lensmodel',
                        required=False,
                        help='''Which lens model we're using. This is a string "LENSMODEL_....". Required
                        unless we have a --seed. See http://mrcal.secretsauce.net/how-to-calibrate.html
                        for notes about how to select a model''')
    parser.add_argument('--focal',
                        type=str,
                        help=r'''Initial estimate of the focal length, in
                        pixels. Required unless --seed is given. See
                        http://mrcal.secretsauce.net/how-to-calibrate.html for
                        notes about how to estimate a focal length. This is
                        either a single value to use for all the cameras, or a
                        comma-separated whitespace-less list of values to use
                        for each camera. If such a list is given, it must match
                        the number of cameras being calibrated''')
    parser.add_argument('--imagersize',
                        nargs=2,
                        type=int,
                        required=False,
                        help='''Size of the imager. This is only required if we pass --corners-cache AND if
                        none of the image files on disk actually exist and if we
                        don't have a --seed. If we do have a --seed, the
                        --imagersize values must match the --seed exactly''')
    parser.add_argument('--outdir',
                        type=lambda d: d if os.path.isdir(d) else \
                                parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
                        default='.',
                        help='Directory for the output camera models')
    parser.add_argument('--object-spacing',
                        required=False,
                        type=float,
                        help='Width of each square in the calibration board, in meters')
    parser.add_argument('--object-width-n',
                        type=int,
                        default=10,
                        help='''How many points the calibration board has per horizontal side. If omitted we
                        default to 10''')
    parser.add_argument('--object-height-n',
                        type=int,
                        help='''How many points the calibration board has per vertical side. If omitted, we
                        assume a square object, setting height=width''')
    parser.add_argument('--seed',
                        required=False,
                        type=str,
                        help='''A comma-separated whitespace-less list of camera model globs to use as a seed
                        for the intrinsics and extrinsics. The number of models
                        must match the number of cameras exactly. Expanded globs
                        are sorted alphanumerically. This is useful to bootstrap
                        the solve or to validate an existing set of models, or
                        to recompute just the extrinsics or just the intrinsics
                        of a solve. If omitted, we estimate a seed. Exclusive
                        with --focal. If given, --imagersize is omitted or it
                        must match EXACTLY with whatever is in the --seed
                        models''')
    parser.add_argument('--jobs', '-j',
                        type=int,
                        default=1,
                        help='''How much parallelization we want. Like GNU make. Affects only the chessboard
                        corner finder. If we are reading a cache file, this does nothing''')
    parser.add_argument('--corners-cache',
                        type=lambda f: f if os.path.isfile(f) or not os.path.isdir(f) else \
                                parser.error(f"--corners-cache requires an existing, readable file as the arg or a non-existing path, but got '{f}'"),
                        required=False,
                        help=r'''Path to the corner-finder results. If this file exists, I use the corners in
                        this file. If it doesn't exist, I invoke mrgingham to
                        compute the corners, and I write the results to that
                        path. And THEN I compute the calibration off those
                        observations. This file is a vnlog with legend "#
                        filename x y level" (exactly what mrgingham reports).
                        Each rown is an observed corners. If an image had no
                        observations, a single row "filename - - -" is expected.
                        The "level" is the decimation level used in detecting
                        that corner. "0" means "full-resolution", "1" means
                        "half-resolution", "2" means "quarter-resolution" and so
                        on. A level of "-" or <0 means "skip this point". This
                        is how incomplete board observations are specified. A
                        file with a missing "level" column will fill in "0" for
                        all corners. A non-mrgingham grid detector may be used
                        by running that separately, and using this option to
                        read the output. A detector may output weights instead
                        of a decimation level in the last column. Pass
                        --corners-cache-has-weights to interpret the data in
                        that way''')
    parser.add_argument('--corners-cache-has-weights',
                        action='store_true',
                        help='''By default the corners we read in --corners-cache have columns "filename x y
                        level". If the last column is a weight instead of a
                        decimation level, pass this option. This is useful to
                        support non-mrgingham chessboard detectors''')

    parser.add_argument('--pairs',
                        action='store_true',
                        help='''By default, we are calibrating a set of N independent cameras. If we actually
                        have a number of stereo pairs, pass this argument. It
                        changes the filename format of the models written to
                        disk (cameraPAIR-INDEXINPAIR.cameramodel), and will
                        report some uncertainties about geometry inside each
                        pair. Consecutive cameras in the given list are paired
                        up, and an even number of cameras is required''')
    parser.add_argument('--skip-regularization',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''By default we apply regularization in the solver in the final optimization.
                        This discourages obviously-wrong solutions, but can
                        introduce a bias. With this option, regularization isn't
                        applied''')
    parser.add_argument('--skip-outlier-rejection',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''By default we throw out outliers. This option turns that off''')
    parser.add_argument('--skip-extrinsics-solve',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''Keep the seeded extrinsics, if given. Allowed only if --seed''')
    parser.add_argument('--skip-intrinsics-solve',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''Keep the seeded intrinsics, if given. Allowed only if --seed''')
    parser.add_argument('--skip-calobject-warp-solve',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''By default we assume the calibration target is slightly deformed, and we
                        compute this deformation. If we want to assume that it
                        is flat, pass this option.''')

    parser.add_argument('--valid-intrinsics-region-parameters',
                        nargs = 5,
                        default = (1, 0.5, 1.5, 3, 0),
                        type = float,
                        help='''For convenience we compute a valid-intrinsics region to describe the results
                        of the calibration. This is a watered-down
                        interpretation of the projection uncertainty that is
                        easy to interpret. The logic computing this is somewhat
                        crude, and may go away in the future. The defaults
                        should be reasonable, so if in doubt, leave these alone.
                        The intent is to produce usable output even if we're
                        using a lean lens model where the computed uncertainty
                        is always overly optimistic. We bin the observations
                        into a grid, and use mrcal._report_regional_statistics()
                        to get the residual statistics in each bin. We then
                        contour the bins to produce the valid-intrinsics region.
                        If we're using a rich lens model
                        (LENSMODEL_SPLINED_...), then we only look at the
                        uncertainty, and not at the other statistics. This
                        argument takes 5 parameters. The uncertainty is computed
                        at a range valid_intrinsics_region_parameters[4]. If <=
                        0, I look out to infinity. The default is 0. A region is
                        valid only if the projection uncertainty <
                        valid_intrinsics_region_parameters[0] *
                        observed_pixel_uncertainty. The default is 1. A region
                        is valid only if the mean-abs-residuals is <
                        valid_intrinsics_region_parameters[1] (only for lean
                        models). The default is 0.5. A region is valid only if
                        the residuals stdev is <
                        valid_intrinsics_region_parameters[2] *
                        observed_pixel_uncertainty (only for lean models). The
                        default is 1.5. A region is valid only if it contains at
                        least valid_intrinsics_region_parameters[3] observations
                        (only for lean models). The default is 3.''')

    parser.add_argument('--verbose-solver',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''By default the final stage of the solver doesn't say much. This option turns
                        on verbosity to get lots of diagnostics. This is
                        generally not very useful to end users''')

    parser.add_argument('--explore',
                        action='store_true',
                        required=False,
                        default=False,
                        help='''After the solve open an interactive shell to examine the solution''')

    parser.add_argument('images',
                        type=str,
                        nargs='+',
                        help='''A glob-per-camera for the images. Include a glob for each camera. It is
                        assumed that the image filenames in each glob are of of
                        the form xxxNNNyyy where xxx and yyy are common to all
                        images in the set, and NNN varies. This NNN is a frame
                        number, and identical frame numbers across different
                        globs signify a time-synchronized observation. I.e. you
                        can pass 'left*.jpg' and 'right*.jpg' to find images
                        'left0.jpg', 'left1.jpg', ..., 'right0.jpg',
                        'right1.jpg', ...''')

    return parser.parse_args()

args = parse_args()

# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README


if args.object_spacing is None:
    print("WARNING: assuming default calibration-object spacing of 0.1m. If this is wrong, all distances will be off by a scale factor",
          file = sys.stderr)
    args.object_spacing = 0.1

if args.object_height_n is None:
    args.object_height_n = args.object_width_n




import numpy as np
import scipy.linalg
import numpysane as nps
import copy
import time
import glob

import mrcal

# wider printing is more convenient here
np.set_printoptions(linewidth=300)


def get_imagersize_one(icamera, indices_frame_camera, paths, args_imagersize, seedmodels):
    r'''Returns the imager size for a given camera

    This reports the size for ONE camera. I only look at the first match. It is
    assumed that all the images matching this glob have the same imager size.

    If I have a corners cache, then this is the ONLY place where I'd need the
    images on disk at all. If the user passes --imagersize, then I really don't
    need the images.

    '''
    if args_imagersize is not None:
        return args_imagersize
    if seedmodels is not None:
        return seedmodels[icamera].imagersize()

    try:
        iobservation0_thiscamera = next( i for i in range(len(paths)) if indices_frame_camera[i,1] == icamera )
    except:
        raise Exception(f"Couldn't find any images for camera '{icamera}'")

    try:
        img = mrcal.load_image(paths[iobservation0_thiscamera],
                               bits_per_pixel = 8, channels = 1)
    except:
        raise Exception(f"I needed to read '{paths[iobservation0_thiscamera]}' to get an imagersize, but couldn't open it, and get image dimensions from it. Make the images findable, or pass --imagersize")
    h,w = img.shape[:2]

    return [w,h]


def solve_initial(args, seedmodels, imagersizes,
                  observations, indices_frame_camera):
    '''Solve an incrementally-expanding optimization problem in several passes

    observations[...,2] start out as the initial outlier set, and are modified
    by this function to represent the expanded outlier set

    '''

    indices_frame_camintrinsics_camextrinsics = \
        nps.glue(indices_frame_camera,
                 indices_frame_camera[:,(1,)]-1,
                 axis=-1)

    if seedmodels is None:
        # I have no seed. I compute a rough seed, and run a few preliminary,
        # incremental optimizations to get it reasonably-close to the right
        # answer
        intrinsics_data,extrinsics_rt_fromref,frames_rt_toref = \
            mrcal.seed_stereographic(imagersizes          = imagersizes,
                                     focal_estimate       = args.focal,
                                     indices_frame_camera = indices_frame_camera,
                                     observations         = observations,
                                     object_spacing       = args.object_spacing)

        sys.stderr.write("## initial solve: geometry only\n")
        lensmodel = 'LENSMODEL_STEREOGRAPHIC'
        stats = mrcal.optimize(intrinsics_data, extrinsics_rt_fromref, frames_rt_toref, None,
                               observations, indices_frame_camintrinsics_camextrinsics,
                               None, None,
                               lensmodel,
                               imagersizes                       = imagersizes,
                               do_optimize_intrinsics_core       = False,
                               do_optimize_intrinsics_distortions= False,
                               do_optimize_calobject_warp        = False,
                               calibration_object_spacing        = args.object_spacing,
                               do_apply_outlier_rejection        = False,
                               do_apply_regularization           = False,
                               verbose                           = False)
        sys.stderr.write(f"## RMS error: {stats['rms_reproj_error__pixels']}\n\n")

        sys.stderr.write("## initial solve: geometry and LENSMODEL_STEREOGRAPHIC core only\n")
        stats = mrcal.optimize(intrinsics_data, extrinsics_rt_fromref, frames_rt_toref, None,
                               observations, indices_frame_camintrinsics_camextrinsics,
                               None, None,
                               lensmodel,
                               imagersizes                       = imagersizes,
                               do_optimize_intrinsics_core       = True,
                               do_optimize_intrinsics_distortions= False,
                               do_optimize_calobject_warp        = False,
                               calibration_object_spacing        = args.object_spacing,
                               do_apply_outlier_rejection        = False,
                               do_apply_regularization           = False,
                               verbose                           = False)

        extrinsics_Rt_fromref = mrcal.Rt_from_rt( extrinsics_rt_fromref )

    else:

        # The caller made sure that all the models use the same lens model
        lensmodel = seedmodels[0].intrinsics()[0]
        intrinsics_data  = nps.cat( *[m.intrinsics()[1] for m in seedmodels])

        # I keep the relative camera poses constant, but place camera0 at the
        # origin
        Rt_r0 = seedmodels[0].extrinsics_Rt_toref()
        extrinsics_Rt_fromref = \
            nps.cat( *[ mrcal.compose_Rt( m.extrinsics_Rt_fromref(),Rt_r0)
                        for m in seedmodels[1:]])

        if extrinsics_Rt_fromref.size == 0:
            # No extrinsics. Represent as 0 Rt transforms
            extrinsics_Rt_fromref = np.zeros((0,4,3), dtype=float)

        # I have a GOOD extrinsics estimate, so I could compute a GOOD frame
        # pose estimate by triangulating:
        #
        #   import deltapose_lite
        #   for corresponding images:
        #       for points in chessboard:
        #           lindstrom to get point in 3d
        #           procrustes fit to get frame transformation
        #
        # But this takes a lot of typing, and wouldn't handle special cases:
        # - what if for a given frame only 1 camera is observing the board?
        # - what if for a given frame more than 2 cameras are observing the board?
        #
        # So I do the less-accurate-but-more-robust thing using pinhole
        # monocular observations. This is what mrcal.seed_stereographic()
        # does in the no-seed-available case
        calobject_poses_local_Rt_cf = \
            mrcal.estimate_monocular_calobject_poses_Rt_tocam( indices_frame_camera,
                                                               observations,
                                                               args.object_spacing,
                                                               seedmodels)
        frames_rt_toref = \
            mrcal.estimate_joint_frame_poses(
                calobject_poses_local_Rt_cf, extrinsics_Rt_fromref,
                indices_frame_camera,
                args.object_width_n,args.object_height_n,
                args.object_spacing)

    def expand_intrinsics(lensmodel, intrinsics_data):
        NnewDistortions = \
            mrcal.lensmodel_num_params(lensmodel) - \
            intrinsics_data.shape[-1]
        newDistortions = \
            (np.random.random((Ncameras, NnewDistortions)) - 0.5)*2. *1e-6
        m = re.search("OPENCV([0-9]+)", lensmodel)
        if m:
            Nd = int(m.group(1))
            if Nd >= 8:
                # Push down the rational components of the seed. I'd like these all to
                # sit at 0 ideally. The radial distortion in opencv is x_distorted =
                # x*scale where r2 = norm2(xy - xyc) and
                #
                # scale = (1 + k0 r2 + k1 r4 + k4 r6)/(1 + k5 r2 + k6 r4 + k7 r6)
                #
                # Note that k2,k3 are tangential (NOT radial) distortion components.
                # Note that the r6 factor in the numerator is only present for
                # >=LENSMODEL_OPENCV5. Note that the denominator is only present for >=
                # LENSMODEL_OPENCV8. The danger with a rational model is that it's
                # possible to get into a situation where scale ~ 0/0 ~ 1. This would
                # have very poorly behaved derivatives. If all the rational coefficients
                # are ~0, then the denominator is always ~1, and this problematic case
                # can't happen. I favor that.
                newDistortions[5:8] *= 1e-3
        return nps.glue( intrinsics_data, newDistortions, axis=-1 )


    # Alrighty. All the preliminary business is finished. I should have a usable
    # seed now. And thus I now run the main optimization loop
    lensmodel = args.lensmodel
    intrinsics_data  = expand_intrinsics(lensmodel, intrinsics_data)

    print("=================== optimizing everything{}from seeded intrinsics". \
          format(" except board warp " if not args.skip_calobject_warp_solve else " "))

    extrinsics_rt_fromref = mrcal.rt_from_Rt(extrinsics_Rt_fromref)

    # splined models have a core, but those variables are largely redundant with
    # the spline parameters. So I run another pre-solve to get reasonable values
    # for the core, and then I lock it down
    do_optimize_intrinsics_core = not args.skip_intrinsics_solve
    if re.match("LENSMODEL_SPLINED_STEREOGRAPHIC_", lensmodel):
        do_optimize_intrinsics_core = False

    optimization_inputs = \
        dict( intrinsics                                = intrinsics_data,
              extrinsics_rt_fromref                     = extrinsics_rt_fromref,
              frames_rt_toref                           = frames_rt_toref,
              points                                    = None,
              observations_board                        = observations,
              indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
              observations_point                        = None,
              indices_point_camintrinsics_camextrinsics = None,
              lensmodel                                 = lensmodel,
              imagersizes                               = imagersizes,
              calobject_warp                            = None,
              do_optimize_intrinsics_core               = do_optimize_intrinsics_core,
              do_optimize_intrinsics_distortions        = not args.skip_intrinsics_solve,
              do_optimize_extrinsics                    = not args.skip_extrinsics_solve,
              do_optimize_frames                        = True,
              do_optimize_calobject_warp                = False,
              calibration_object_spacing                = args.object_spacing,
              do_apply_outlier_rejection                = not args.skip_outlier_rejection,
              do_apply_regularization                   = True,
              verbose                                   = False,
              imagepaths                                = paths )

    mrcal.optimize( **optimization_inputs )
    return optimization_inputs







# expand ~/ into $HOME/
args.images = [os.path.expanduser(g) for g in args.images]

Ncameras = len(args.images)
if Ncameras > 10:
    raise Exception("Got {} image globs. It should be one glob per camera, and this sounds like WAY too many cameras. Did you forget to escape your glob?". \
                    format(Ncameras))

try:
    args.focal = [ float(f.strip()) for f in args.focal.split(',') ]
except:
    print(f"--focal must be given a positive floating-point value, or a comma-separated list of such values",
          file=sys.stderr)
    sys.exit(1)
if not all(f>0 for f in args.focal):
    print("--focal must be given a POSITIVE floating-point value, or a comma-separated list of such values. Some weren't positive",
          file=sys.stderr)
    sys.exit(1)

if not (len(args.focal) == 1 or \
        len(args.focal) == Ncameras):
    print(f"--focal must be a single value, or a comma-separated list of exactly Ncameras such values. Received Ncameras={Ncameras} image globs and {len(args.focal)} focal lengths",
          file=sys.stderr)
    sys.exit(1)


if args.pairs and Ncameras % 2:
    raise Exception(f"With --pairs I must have gotten an even number of cameras, but instead got {Ncameras}")

if args.seed:

    if args.focal is not None:
        raise Exception("Exactly one of --focal and --seed MUST be given")


    def seedmodels_iterator():
        for g in args.seed.split(','):
            globbed_filenames = sorted(glob.glob(g))
            if 0 == len(globbed_filenames):
                raise Exception(f"seed glob '{g}' matched no files!")
            for f in globbed_filenames:
                yield mrcal.cameramodel(f)
    seedmodels = list(seedmodels_iterator())

    if Ncameras != len(seedmodels):
        raise Exception(f"I saw {Ncameras} image globs, but {len(seedmodels)} --seed models. Both represent cameras, so I should have identical counts")

    if args.imagersize is not None:
        for m in seedmodels:
            if args.imagersize != list(m.imagersize()):
                raise Exception("Both --seed and --imagersize were given, so they must match exactly. But I had --imagersize {} and one model has imagersize() = {}". \
                                format(args.imagersize, m.imagersize()))
    lensmodel = seedmodels[0].intrinsics()[0]
    for m in seedmodels[1:]:
        if lensmodel != m.intrinsics()[0]:
            raise Exception("I expect all cameras to use the same lens model, but --seed saw {} and {}". \
                            format(lensmodel,
                                   m.intrinsics()[0]))

    if args.lensmodel is None:
        args.lensmodel = lensmodel


else:
    if args.focal is None:
        raise Exception("Exactly one of --focal and --seed MUST be given")

    if not args.lensmodel:
        raise Exception("--lensmodel is required if no --seed")

    if args.skip_extrinsics_solve:
        raise Exception("--skip-extrinsics-solve requires --seed")
    if args.skip_intrinsics_solve:
        raise Exception("--skip-intrinsics-solve requires --seed")

    seedmodels = None



observations, indices_frame_camera, paths = \
    mrcal.compute_chessboard_corners(args.object_width_n,
                                     args.object_height_n,
                                     globs_per_camera   = args.images,
                                     corners_cache_vnl  = args.corners_cache,
                                     jobs               = args.jobs,
                                     weight_column_kind = 'weight' if args.corners_cache_has_weights else 'level')

Nobservations = len(observations)

# list of imager sizes; one per camera
imagersizes = np.array([get_imagersize_one(icamera,
                                           indices_frame_camera,
                                           paths,
                                           args.imagersize,
                                           seedmodels) for icamera in range(Ncameras)],
                       dtype=np.int32)

optimization_inputs = \
    solve_initial(args, seedmodels,
                  imagersizes,
                  observations, indices_frame_camera)

if not args.skip_calobject_warp_solve:
    calobject_warp = np.array((0,0), dtype=float)
else:
    calobject_warp = None

print("## final, full optimization",
      file=sys.stderr)

optimization_inputs['calobject_warp']             = calobject_warp
optimization_inputs['do_optimize_calobject_warp'] = not args.skip_calobject_warp_solve
optimization_inputs['do_apply_regularization']    = not args.skip_regularization
optimization_inputs['do_apply_outlier_rejection'] = not args.skip_outlier_rejection
optimization_inputs['verbose']                    = args.verbose_solver

# If we're skipping the regularization step, I do EVERYTHING else before the
# final regularization-free step. Turning that off allows the solution to
# wander, and I want to help it as much as possible to not do that
if not optimization_inputs['do_apply_regularization']:
    optimization_inputs['do_apply_regularization']= True
    optimization_inputs['verbose']                = False

    stats = mrcal.optimize(**optimization_inputs)

    optimization_inputs['do_apply_regularization']= False
    optimization_inputs['verbose']                = args.verbose_solver

stats = mrcal.optimize(**optimization_inputs)
sys.stderr.write(f"## RMS error: {stats['rms_reproj_error__pixels']}\n")

report = f"RMS reprojection error: {stats['rms_reproj_error__pixels']:.01f} pixels\n"

Npoints_chessboard = args.object_width_n*args.object_height_n*Nobservations

# shape (Nobservations,Nheight,Nwidth,2)
residuals = \
    stats['x'][:Npoints_chessboard*2]. \
    reshape(Nobservations, args.object_height_n, args.object_width_n, 2)
worst_point_err = np.sqrt(np.max(nps.norm2( nps.clump(residuals, n=3) )))
report += f"Worst residual (by measurement): {worst_point_err:.01f} pixels\n"
if not args.skip_outlier_rejection:
    report += "Noutliers: {} out of {} total points: {:.01f}% of the data\n". \
        format(stats['Noutliers'],
               args.object_height_n*args.object_width_n*len(observations),
               100.0 * stats['Noutliers'] / (args.object_height_n*args.object_width_n*len(observations)))
if calobject_warp is not None:
    report += f"calobject_warp = {calobject_warp}\n"

print(report)

models = \
    [ mrcal.cameramodel( \
                optimization_inputs = optimization_inputs,
                icam_intrinsics     = icam ) \
      for icam in range(len(optimization_inputs['intrinsics'])) ]

# shape (Nobservations)
nonoutliers_per_observation = \
    np.sum( nps.clump( (observations[...,2] > 0.).astype(int),
                       n = -2 ),
            axis = -1 )

# shape (Nobservations,)
rms_residual_perobservation = \
    np.sqrt( nps.norm2( nps.clump(residuals,n=-3) ) /
             nonoutliers_per_observation )

# shape (Nobservations,)
i_observations_sorted_from_worst = \
    list(reversed(np.argsort(rms_residual_perobservation)))


if not args.skip_intrinsics_solve and \
   args.valid_intrinsics_region_parameters is not None:

    observed_pixel_uncertainty = \
        np.std(mrcal.residuals_chessboard(optimization_inputs,
                                          residuals = stats['x']).ravel())

    threshold_uncertainty = args.valid_intrinsics_region_parameters[0] * observed_pixel_uncertainty
    threshold_mean        = args.valid_intrinsics_region_parameters[1]
    threshold_stdev       = args.valid_intrinsics_region_parameters[2] * observed_pixel_uncertainty
    threshold_count       = args.valid_intrinsics_region_parameters[3]
    distance              = args.valid_intrinsics_region_parameters[4]

    for i in range(Ncameras):
        models[i].valid_intrinsics_region( \
            mrcal.calibration._compute_valid_intrinsics_region(models[i],
                                                   threshold_uncertainty,
                                                   threshold_mean,
                                                   threshold_stdev,
                                                   threshold_count,
                                                   distance))


# The note says how we ran this, and contains the commented-out report
note = \
    f"generated on {time.strftime('%Y-%m-%d %H:%M:%S')} with   {' '.join(mrcal.shellquote(s) for s in sys.argv)}\n" + \
    report
for icam in range(len(models)):

    filename_base = \
        f'{args.outdir}/camera{icam//2}-{icam%2}' \
        if args.pairs \
        else f'{args.outdir}/camera-{icam}'

    cameramodelfile = filename_base + '.cameramodel'
    models[icam].write(cameramodelfile,
                       note = note)
    print(f"Wrote {cameramodelfile}")



if not args.explore:
    sys.exit(0)



# We're exploring!
import gnuplotlib as gp


print('\n')
print(r'''Solution-examination REPL.
Potential things to look at:

    show_geometry()
    show_residuals_board_observation_worst(i_observation_in_order_from_worst)
    show_residuals_board_observation(i_observation, vectorscale=20)
    show_projection_uncertainty(icam)
    show_valid_intrinsics_region(icam)
    show_residuals_vectorfield(    icam)
    show_residuals_magnitudes(     icam)
    show_residuals_directions(     icam)
    show_residuals_histogram(      icam)
    show_residuals_regional(       icam)
    show_distortion_off_pinhole(   icam, vectorfield=False)
    show_distortion_off_pinhole_radial(icam)
    show_splined_model_correction(icam)
    stats
    i_observations_sorted_from_worst
    rms_residual_perobservation
    paths[i_observations_sorted_from_worst[0]]
    calobject_warp
''')



# I want multiple plots to be able to be shown at once. I store the gnuplotlib
# objects into values in this dict
plots = dict()

def show_generic_per_cam(f, icam, **kwargs):
    '''Generic show-things function that plots one camera/plot'''
    global plots
    icam_all = (icam,) if icam is not None else range(Ncameras)
    for icam in icam_all:
        plots[f.__name__ + str(icam)] = \
            f(models[icam],
              extratitle = f"camera {icam}",
              **kwargs)

def show_generic(f, *args, **kwargs):
    '''Generic show-things function that plots all cameras/plot'''
    global plots
    plots[f.__name__] = f(*args, **kwargs)

def show_splined_model_correction     (icam = None, **kwargs): show_generic_per_cam(mrcal.show_splined_model_correction,     icam, **kwargs)
def show_projection_uncertainty        (icam = None, **kwargs): show_generic_per_cam(mrcal.show_projection_uncertainty,        icam, **kwargs)
def show_valid_intrinsics_region       (icam = None, **kwargs): show_generic_per_cam(mrcal.show_valid_intrinsics_region,       icam, **kwargs)

def show_residuals_vectorfield (icam = None, **kwargs): show_generic_per_cam(mrcal.show_residuals_vectorfield, icam, residuals=stats['x'], **kwargs)
def show_residuals_magnitudes  (icam = None, **kwargs): show_generic_per_cam(mrcal.show_residuals_magnitudes,  icam, residuals=stats['x'], **kwargs)
def show_residuals_directions  (icam = None, **kwargs): show_generic_per_cam(mrcal.show_residuals_directions,  icam, residuals=stats['x'], **kwargs)
def show_residuals_regional    (icam = None, **kwargs): show_generic_per_cam(mrcal.show_residuals_regional,    icam, residuals=stats['x'], **kwargs)

def show_distortion_off_pinhole       (icam = None, **kwargs): show_generic_per_cam(mrcal.show_distortion_off_pinhole,        icam, **kwargs)
def show_distortion_off_pinhole_radial(icam = None, **kwargs): show_generic_per_cam(mrcal.show_distortion_off_pinhole_radial, icam, **kwargs)

def show_geometry(**kwargs): show_generic(mrcal.show_geometry,
                                          models, **kwargs)

def show_residuals_board_observation(i_observation, **kwargs):
    return show_generic( mrcal.show_residuals_board_observation,
                         optimization_inputs,
                         i_observation,
                         paths                            = paths,
                         i_observations_sorted_from_worst = i_observations_sorted_from_worst,
                         residuals                        = stats['x'],
                         **kwargs)


def show_residuals_board_observation_worst(i_observation_from_worst, **kwargs):
    return show_generic( mrcal.show_residuals_board_observation,
                         optimization_inputs,
                         i_observation_from_worst,
                         from_worst                       = True,
                         paths                            = paths,
                         i_observations_sorted_from_worst = i_observations_sorted_from_worst,
                         residuals                        = stats['x'],
                         **kwargs)

def show_residuals_histogram(icam = None, **kwargs):
    show_generic(mrcal.show_residuals_histogram,
                 optimization_inputs,
                 icam,
                 residuals=stats['x'],
                 **kwargs)

import IPython
IPython.embed()
