#!/usr/bin/env 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


r'''Converts a camera model from one lens model to another

SYNOPSIS

  $ mrcal-convert-lensmodel
      --viz LENSMODEL_OPENCV4 left.cameramodel

  ... lots of output as the solve runs ...
  RMS error of the solution: 3.40256580058 pixels.
  Wrote 'left-LENSMODEL_OPENCV4.cameramodel'

  ... a plot pops up showing the differences ...

Given one camera model, this tool computes another camera model that represents
the same camera, but utilizes a different lens model. While lens models all
exist to solve the same problem, the different representations don't map to one
another perfectly, and this tool finds the best-fitting parameters of the target
lens model. Two different methods are implemented:

1. If the given cameramodel file contains optimization_inputs, then we have all
   the data that was used to compute this model in the first place, and we can
   re-run the original optimization, using the new lens model. This is the
   default behavior, and is the preferred choice. However it can only work with
   models that were computed by mrcal originally. We re-run the full original
   solve, even it contained multiple cameras, unless --monocular is given. With
   that option, we re-solve only the subset of the images observed by the one
   requested camera

2. We can sample a grid of points on the imager, unproject them to observation
   vectors in the camera coordinate system, and then fit a new camera model that
   reprojects these vectors as closely to the original pixel coordinates as
   possible. This can be applied to models that didn't come from mrcal. Select
   this mode by passing --sampled.

Since camera models (lens parameters AND geometry) are computed off real pixel
observations, the confidence of the projections varies greatly across the imager
and across observation distances. The first method uses the original data, so it
implicitly respects these uncertainties: uncertain areas in the original model
will be uncertain in the new model as well. The second method, however, doesn't
have this information: it doesn't know which parts of the imager and space are
reliable, so the results suffer.

As always, the intrinsics have some baked-in geometry information. Both methods
optimize intrinsics AND extrinsics, and output cameramodels with updated
versions of both. If --sampled: we can request that only the intrinsics be
optimized by passing --intrinsics-only.

Also, if --sampled and not --intrinsics-only: we fit the extrinsics off 3D
points, not just observation directions. The distance from the camera to the
points is set by --distance. This can take a comma-separated list of distances
to use. It's STRONGLY recommended to ask for two different distances:

- A "near" distance: where we expect the intrinsics to have the most accuracy.
  At the range of the chessboards, usually

- A "far" distance: at "infinity". A few km is good usually.

The reason for this is that --sampled solves at a single distance aren't
sufficiently constrained. If we ask for a single far distance: "--distance 1000"
for instance, we can easily get an extrinsics shift of 100m. This is aphysical:
changing the intrinsics could shift the camera origin by a few mm, but not 100m.
Conceptually we want to perform a rotation-only extrinsics solve, but this isn't
yet implemented. Passing both a near and far distance appears to constrain the
extrinsics well in practice. The computed extrinsics transform is printed on the
console, with a warning if an aphysical shift was computed. Do pay attention to
the console output.

Sampled solves are sometimes sensitive to the optimization seed. To control for
this, pass --num-trials to evaluate the solve multiple times from different
random seeds, and to pick the best one. These solves are usually quick, so
there's no harm in passing something like "--num-trials 10".

We need to consider that the model we're trying to fit may not fit the original
model in all parts of the imager. Usually this is a factor when converting
wide-angle cameramodels to use a leaner model: a decent fit will be possible at
the center, with more and more divergence as we move towards the edges. We
handle this with the --where and --radius options to allow the user to select
the area of the imager that is used for the fit: observations outside the
selected area are thrown out. This region is centered on the point given by
--where (or at the center of the imager, if --where is omitted). The radius of
this region is given by --radius. If '--radius 0' then we use ALL the data. A
radius<0 can be used to set the size of the no-data margin at the corners; in
this case I'll use

    r = sqrt(width^2 + height^2)/2. - abs(radius)

There's a balance to strike here. A larger radius means that we'll try to fit as
well as we can in a larger area. This might mean that we won't fit well
anywhere, but we won't do terribly anywhere, either. A smaller area means that
we give up on the outer regions entirely (resulting in very poor fits there),
but we'll be able to fit much better in the areas that remain. Generally
empirical testing is required to find a good compromise: pass --viz to see the
resulting differences. Note that --radius and --where applies only if we're
optimizing sampled reprojections; if we're using the original optimization
inputs, the options are illegal.

The output is written to a file on disk, with the same filename as the input
model, but with the new lensmodel added to the filename.

'''



import sys
import argparse
import re
import os

def parse_args():

    parser = \
        argparse.ArgumentParser(description = __doc__,
                                formatter_class=argparse.RawDescriptionHelpFormatter)

    parser.add_argument('--sampled',
                        action='store_true',
                        help='''Instead of solving the original calibration problem using the new lens model,
                        use sampled imager points. This produces biased results,
                        but can be used even if the original optimization_inputs
                        aren't available''')
    parser.add_argument('--gridn',
                        type=int,
                        default = (30,20),
                        nargs = 2,
                        help='''Used if --sampled. How densely we should sample the imager. By default we use
                        a 30x20 grid''')
    parser.add_argument('--distance',
                        type=str,
                        help='''Required if --sampled and not --intrinsics-only.
                        A sampled solve fits the intrinsics and extrinsics to
                        match up reprojections of a grid of observed pixels. The
                        points being projected are set a particular distance
                        (set by this argument) from the camera. Set this to the
                        distance that is expected to be most confident for the
                        given cameramodel. Points at infinity aren't supported
                        yet: specify a high distance instead. We can fit
                        multiple distances at the same time by passing them here
                        in a comma-separated, whitespace-less list. If multiple
                        distances are given, we fit the model using ALL the
                        distances, but --viz displays the difference for the
                        FIRST distance given. See the description above. Without
                        --sampled, this is used for the visualization only''')
    parser.add_argument('--intrinsics-only',
                        action='store_true',
                        help='''Used if --sampled. By default I optimize the
                        intrinsics and extrinsics to find the closest
                        reprojection. If for whatever reason we know that the
                        camera coordinate system was already right, or we need
                        to keep the original extrinsics, pass --intrinsics-only.
                        The resulting extrinsics will be the same, but the fit
                        will not be as good. In many cases, optimizing
                        extrinsics is required to get a usable fit, so
                        --intrinsics-only may not be an option if accurate
                        results are required.''')

    parser.add_argument('--where',
                        type=float,
                        nargs=2,
                        help='''Used with or without --sampled. I use a subset
                        of the imager to compute the fit. The active region is a
                        circle centered on this point. If omitted, we will focus
                        on the center of the imager''')
    parser.add_argument('--radius',
                        type=float,
                        help='''Used with or without --sampled. I use a subset
                        of the imager to compute the fit. The active region is a
                        circle with a radius given by this parameter. If radius
                        == 0, I'll use the whole imager for the fit. If radius <
                        0, this parameter specifies the width of the region at
                        the corners that I should ignore: I will use
                        sqrt(width^2 + height^2)/2. - abs(radius). This is valid
                        ONLY if we're focusing at the center of the imager. By
                        default I ignore a large-ish chunk area at the corners.''')

    parser.add_argument('--monocular',
                        action='store_true',
                        help='''Used if not --sampled. By default we re-solve
                        the FULL calibration problem that was used to originally
                        obtain the input model, even if it contained multiple
                        cameras. If --monocular, we re-solve only a subset of
                        the original problem, using ONLY the observations made
                        by THIS camera. Exclusive with --all''')
    parser.add_argument('--all',
                        action='store_true',
                        help='''Used if not --sampled. By default we re-solve
                        the FULL calibration problem that was used to originally
                        obtain the input model, even if it contained multiple
                        cameras. And by default, we only output the result for
                        the ONE given model. If --all, we output the result for
                        ALL the models in the solve. Exclusive with --monocular''')

    parser.add_argument('--viz',
                        action='store_true',
                        help='''Visualize the differences between the input and
                        output models. If we have --distance, the FIRST given
                        distance is used to display the fit error''')
    parser.add_argument('--cbmax',
                        type=float,
                        default=4,
                        help='''Maximum range of the colorbar''')
    parser.add_argument('--title',
                        type=str,
                        default = None,
                        help='''Used if --viz. Title string for the diff plot.
                        Overrides the default title. Exclusive with
                        --extratitle''')
    parser.add_argument('--extratitle',
                        type=str,
                        default = None,
                        help='''Used if --viz. Additional string for the plot to
                        append to the default title. Exclusive with --title''')
    parser.add_argument('--hardcopy',
                        type=str,
                        help='''Used if --viz. Write the diff output to disk,
                        instead of making an interactive plot''')
    parser.add_argument('--terminal',
                        type=str,
                        help=r'''Used if --viz. gnuplotlib terminal. The default
                        is good almost always, so most people don't need this
                        option''')
    parser.add_argument('--set',
                        type=str,
                        action='append',
                        help='''Used if --viz. Extra 'set' directives to
                        gnuplotlib. Can be given multiple times''')
    parser.add_argument('--unset',
                        type=str,
                        action='append',
                        help='''Used if --viz. Extra 'unset' directives to
                        gnuplotlib. Can be given multiple times''')

    parser.add_argument('--force', '-f',
                        action='store_true',
                        default=False,
                        help='''By default existing models on disk are not
                        overwritten. Pass --force to overwrite them without
                        complaint''')
    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}'"),
                        help='''Directory to write the output into. If omitted,
                        we use the directory of the input model. Ignored if
                        we're reading stdin (model is omitted or "-"''')

    parser.add_argument('--num-trials',
                        type    = int,
                        default = 1,
                        help='''If given, run the solve more than once. Useful
                        in case random initialization produces noticeably
                        different results. By default we run just one trial''')

    parser.add_argument('to',
                        type=str,
                        help='The target lens model')

    parser.add_argument('model',
                        default='-',
                        nargs='?',
                        type=str,
                        help='''Input camera model. If omitted or "-", we read
                        standard input and write to standard output''')

    args = parser.parse_args()

    if args.all and args.monocular:
        print("Error: --monocular and --all are exclusive", file=sys.stderr)
        sys.exit(1)
    if args.sampled:
        if args.monocular:
            print("--monocular doesn't work with --sampled",
                  file=sys.stderr)
            sys.exit(1)
        if args.all:
            print("--all doesn't work with --sampled",
                  file=sys.stderr)
            sys.exit(1)

    if args.title      is not None and \
       args.extratitle is not None:
        print("Error: --title and --extratitle are exclusive", file=sys.stderr)
        sys.exit(1)

    if args.distance is not None:
        import math
        try:
            args.distance = [float(d) for d in args.distance.split(',')]
        except:
            print("Error: distances must be given a comma-separated list of floats in --distance",
                  file=sys.stderr)
            sys.exit(1)

        if any(not math.isfinite(x) or x <= 0 for x in args.distance):
            print("All values in --distances must be finite and > 0",
                  file=sys.stderr)
            sys.exit(1)

    return 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



import numpy as np
import numpysane as nps
import time
import copy

import mrcal

lensmodel_to = args.to

try:
    meta = mrcal.lensmodel_metadata_and_config(lensmodel_to)
except Exception as e:
    print(f"Invalid lens model '{lensmodel_to}': couldn't get the metadata: {e}",
          file=sys.stderr)
    sys.exit(1)
if not meta['has_gradients']:
    print(f"lens model {lensmodel_to} is not supported at this time: its gradients aren't implemented",
          file=sys.stderr)
    sys.exit(1)

try:
    Ndistortions = mrcal.lensmodel_num_params(lensmodel_to) - 4
except:
    print(f"Unknown lens model: '{lensmodel_to}'", file=sys.stderr)
    sys.exit(1)


try:
    m = mrcal.cameramodel(args.model)
except Exception as e:
    print(f"Couldn't load camera model '{args.model}': {e}", file=sys.stderr)
    sys.exit(1)


if args.model == '-':
    # Input read from stdin. Write output to stdout
    file_output = sys.stdout
else:
    # {CAMID} filled in later
    if args.outdir is None:
        filename_base,extension = os.path.splitext(args.model)
        file_output = f"{filename_base}-{lensmodel_to}{{CAMID}}{extension}"
    else:
        f,extension             = os.path.splitext(args.model)
        directory,filename_base = os.path.split(f)
        file_output = f"{args.outdir}/{filename_base}-{lensmodel_to}{{CAMID}}{extension}"

    if os.path.exists(file_output) and not args.force:
        print(f"ERROR: '{file_output}' already exists. Not overwriting this file. Pass -f to overwrite",
              file=sys.stderr)
        sys.exit(1)


lensmodel_from = m.intrinsics()[0]

if lensmodel_from == lensmodel_to:
    print(f"Input and output have the same lens model {lensmodel_from}. Nothing to do", file=sys.stderr)
    print("RMS error of the solution: 0 pixels.", file=sys.stderr)
    sys.exit(0)



def scale_down_rational_intrinsics(intrinsics):
    modelmatch = re.search("OPENCV([0-9]+)", lensmodel_to)
    if modelmatch:
        Nd = int(modelmatch.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.
            intrinsics[...,4:][...,5:8] *= 1e-3



if not args.sampled:

    if args.intrinsics_only:
        print("--intrinsics-only requires --sampled",
              file=sys.stderr)
        sys.exit(1)
    if not (mrcal.lensmodel_metadata_and_config(lensmodel_from)['has_core'] and \
            mrcal.lensmodel_metadata_and_config(lensmodel_to  )['has_core']):
        print("Without --sampled, the TO and FROM models must support contain an intrinsics core. It COULD work otherwise, but somebody needs to implement it",
              file=sys.stderr)
        sys.exit(1)

    optimization_inputs = m.optimization_inputs()
    if optimization_inputs is None:
        print("optimization_inputs not available in this model, so only sampled fits are possible. Pass --sampled",
              file=sys.stderr)
        sys.exit(1)

    icam_intrinsics = m.icam_intrinsics()

    if args.monocular:
        if optimization_inputs['indices_point_camintrinsics_camextrinsics'] is not None and \
           optimization_inputs['indices_point_camintrinsics_camextrinsics'].size > 0:
            print("--monocular can be used ONLY to re-optimize vanilla calibration problems. This one has points",
                  file=sys.stderr)
            sys.exit(1)

        intrinsics = \
            optimization_inputs['intrinsics']
        imagersizes = \
            optimization_inputs['imagersizes']
        rt_cam_ref = \
            optimization_inputs['rt_cam_ref']
        rt_ref_frame = \
            optimization_inputs['rt_ref_frame']
        indices_frame_camintrinsics_camextrinsics = \
            optimization_inputs['indices_frame_camintrinsics_camextrinsics']
        observations_board = \
            optimization_inputs['observations_board']
        if 'imagepaths' in  optimization_inputs:
            imagepaths = \
                optimization_inputs['imagepaths']

        mask_observations = indices_frame_camintrinsics_camextrinsics[:,1] == icam_intrinsics
        indices_frame_camintrinsics_camextrinsics = \
            indices_frame_camintrinsics_camextrinsics[mask_observations]
        observations_board = \
            observations_board[mask_observations]
        if 'imagepaths' in  optimization_inputs:
            imagepaths = \
                imagepaths[mask_observations]

        ### Now I must cull the extrinsics and frames to only use these
        ### observations

        # intrinsics
        indices_frame_camintrinsics_camextrinsics[:,1] = 0
        intrinsics  = intrinsics [(icam_intrinsics,), :]
        imagersizes = imagersizes[(icam_intrinsics,), :]

        # extrinsics
        icam_extrinsics = \
            indices_frame_camintrinsics_camextrinsics[0,2]
        if not np.all(indices_frame_camintrinsics_camextrinsics[:,2] == icam_extrinsics):
            print("Error: --monocular can work ONLY if the calibration-time cameras are stationary. Here the requested camera was moving",
                  file=sys.stderr)
            sys.exit(1)
        indices_frame_camintrinsics_camextrinsics[:,2] = -1
        if icam_extrinsics >= 0:
            # I'm moving the reference to lie at this one new camera. Transform
            # everything
            rt_cam_reforig = rt_cam_ref[icam_extrinsics]
            rt_refnew_reforig = rt_cam_reforig

            rt_ref_frame = mrcal.compose_rt( rt_refnew_reforig, rt_ref_frame )
            rt_cam_ref = np.empty((0,6), dtype=float)

        # frames
        rt_ref_frame = rt_ref_frame[indices_frame_camintrinsics_camextrinsics[:,0], :]
        Nframes = len(indices_frame_camintrinsics_camextrinsics)
        indices_frame_camintrinsics_camextrinsics[:,0] = np.arange(Nframes)

        # store everythin back into the inputs
        optimization_inputs['intrinsics'] = \
            intrinsics
        optimization_inputs['imagersizes'] = \
            imagersizes
        optimization_inputs['rt_cam_ref'] = \
            rt_cam_ref
        optimization_inputs['rt_ref_frame'] = \
            rt_ref_frame
        optimization_inputs['indices_frame_camintrinsics_camextrinsics'] = \
            indices_frame_camintrinsics_camextrinsics
        optimization_inputs['observations_board'] = \
            observations_board
        if 'imagepaths' in  optimization_inputs:
            optimization_inputs['imagepaths'] = \
                imagepaths

        icam_intrinsics = 0


    optimization_inputs_before = copy.deepcopy(optimization_inputs)

    intrinsics_from      = optimization_inputs['intrinsics']
    intrinsics_from_core = intrinsics_from[..., :4]
    Ncameras             = len(intrinsics_from)
    if Ncameras == 1:
        args.all = False

    # Ignore observations in the corners, as requested
    if not (args.radius is None or args.radius == 0):
        for icam in range(Ncameras):

            dims = optimization_inputs['imagersizes'][icam].astype(float)

            if args.where is None:
                focus_center = (dims - 1) / 2
            else:
                focus_center = args.where

            if args.radius > 0:
                r = args.radius
            else:
                if nps.norm2(focus_center - (dims - 1.) / 2) > 1e-3:
                    print("A radius <0 is only implemented if we're focusing on the imager center", file=sys.stderr)
                    sys.exit(1)
                r = nps.mag(dims)/2. + args.radius


            indices_cam  = optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,1]
            observations = optimization_inputs['observations_board']

            mask_thiscam     = indices_cam == icam
            mask_past_radius = nps.norm2(observations[...,:2] - focus_center) > r*r

            observations[nps.mv(mask_thiscam, -1, -3) * mask_past_radius, 2] = -1


    optimization_inputs['verbose']          = False

    optimization_inputs['lensmodel']        = lensmodel_to
    optimization_inputs['intrinsics']       = np.zeros((Ncameras,Ndistortions+4), dtype=float)
    optimization_inputs['intrinsics'][:,:4] = intrinsics_from_core

    # I do this in stages, similar to how mrcal-calibrate-cameras does it. First
    # just the frames and extrinsics. Assuming a core-only intrinsics
    optimization_inputs['do_optimize_intrinsics_core']       = False
    optimization_inputs['do_optimize_intrinsics_distortions']= False
    optimization_inputs['do_optimize_calobject_warp']        = False
    optimization_inputs['do_apply_outlier_rejection']        = False
    optimization_inputs['do_apply_regularization']           = False
    stats = mrcal.optimize(**optimization_inputs)

    # Then I optimize the core also
    optimization_inputs['do_optimize_intrinsics_core']       = True
    stats = mrcal.optimize(**optimization_inputs)

    # Then the intrinsics too
    optimization_inputs['do_optimize_intrinsics_distortions']= True
    optimization_inputs['do_apply_outlier_rejection']        = True
    optimization_inputs['do_apply_regularization']           = True
    if re.match("LENSMODEL_SPLINED_STEREOGRAPHIC_", lensmodel_to):
        # splined models have a core, but those variables are largely redundant
        # with the spline parameters. So I lock down the core when targeting
        # splined models
        optimization_inputs['do_optimize_intrinsics_core'] = False
    # stolen expand_intrinsics() in mrcal-calibrate-extrinsics. Please consolidate
    optimization_inputs['intrinsics'][:,4:] = \
        (np.random.random((Ncameras, Ndistortions)) - 0.5)*2. *1e-6
    scale_down_rational_intrinsics(optimization_inputs['intrinsics'])
    stats = mrcal.optimize(**optimization_inputs)

    # And finally I do the calobject_warp too
    optimization_inputs['do_optimize_calobject_warp'] = True
    stats = mrcal.optimize(**optimization_inputs)


    # I pick the inlier set using the post-solve inliers/outliers. The solve
    # could add some outliers, but it won't take any away, so the union of the
    # before/after inlier sets is just the post-solve set
    idx_inliers_after = optimization_inputs['observations_board'][...,2] > 0

    calobject_camframe_before = \
        mrcal.hypothesis_board_corner_positions(icam_intrinsics,
                                                **optimization_inputs_before,
                                                idx_inliers = idx_inliers_after )[-2]
    calobject_camframe_after = \
        mrcal.hypothesis_board_corner_positions(icam_intrinsics,
                                                **optimization_inputs,
                                                idx_inliers = idx_inliers_after )[-2]

    # This is a procrustes-based transformation. This transform is usable, but
    # isn't based on camera observations, and often produces poor diffs if used
    # directly. Let mrcal.projection_diff() figure out the implied_Rt10 instead
    # of using this
    implied_Rt10 = mrcal.align_procrustes_points_Rt01(calobject_camframe_after,
                                                      calobject_camframe_before)

    # For the purposes of visualization I use what I was given. Or the mean
    # observation distance if I was given nothing
    if args.distance is None:
        distance_for_diff = np.mean(nps.mag(calobject_camframe_after))
    else:
        distance_for_diff = np.array(args.distance)


    print(f"RMS error of the solution: {stats['rms_reproj_error__pixels']} pixels.",
          file=sys.stderr)
    m_to = mrcal.cameramodel( optimization_inputs = optimization_inputs,
                              icam_intrinsics     = icam_intrinsics )

else:

    # Sampled solve. I grid the imager, unproject and fit

    intrinsics_from = m.intrinsics()

    if args.distance is None:
        if args.intrinsics_only:
            # We're only looking at the intrinsics, so the distance doesn't
            # matter. I pick an arbitrary value
            args.distance = [10.]
        else:
            print("--sampled without --intrinsics-only REQUIRES --distance",
                  file=sys.stderr)
            sys.exit(1)

    dims = m.imagersize()
    if dims is None:
        print("Warning: imager size not available. Using centerpixel*2",
              file=sys.stderr)
        dims = intrinsics_from[1][2:4] * 2

    if args.radius is None:
        # By default use 1/4 of the smallest dimension
        args.radius = -np.min(m.imagersize()) // 4
        print(f"Default radius: {args.radius}. We're ignoring the regions {-args.radius} pixels from each corner",
              file=sys.stderr)
        if args.where is not None and \
           nps.norm2(args.where - (dims - 1.) / 2) > 1e-3:
            print("A radius <0 is only implemented if we're focusing on the imager center: use an explicit --radius, or omit --where",
                  file=sys.stderr)
            sys.exit(1)


    # Alrighty. Let's actually do the work. I do this:
    #
    # 1. Sample the imager space with the known model
    # 2. Unproject to get the 3d observation vectors
    # 3. Solve a new model that fits those vectors to the known observations, but
    #    using the new model

    ### I sample the pixels in an NxN grid
    Nx,Ny = args.gridn

    qx = np.linspace(0, dims[0]-1, Nx)
    qy = np.linspace(0, dims[1]-1, Ny)

    # q is (Ny*Nx, 2). Each slice of q[:] is an (x,y) pixel coord
    q = np.ascontiguousarray( nps.transpose(nps.clump( nps.cat(*np.meshgrid(qx,qy)), n=-2)) )
    if args.radius != 0:
        # we use a subset of the input data for the fit
        if args.where is None:
            focus_center = (dims - 1.) / 2.
        else:
            focus_center = args.where

        if args.radius > 0:
            r = args.radius
        else:
            if nps.norm2(focus_center - (dims - 1.) / 2) > 1e-3:
                print("A radius <0 is only implemented if we're focusing on the imager center",
                      file=sys.stderr)
                sys.exit(1)
            r = nps.mag(dims)/2. + args.radius

        grid_off_center = q - focus_center
        i = nps.norm2(grid_off_center) < r*r
        q = q[i, ...]


    # To visualize the sample grid:
    # import gnuplotlib as gp
    # gp.plot(q[:,0], q[:,1], _with='points pt 7 ps 2', xrange=[0,3904],yrange=[3904,0], wait=1, square=1)
    # sys.exit()

    intrinsics_from = m.intrinsics()

    ### I unproject this, with broadcasting
    # shape (Ndistances,)
    d = np.array(args.distance)
    # shape (Ndistances, Ny*Nx, 2)
    q = q * np.ones((d.size,1,1))
    # shape (Ndistances, Ny*Nx, 3)
    p = mrcal.unproject( q, *intrinsics_from, normalize = True ) * \
        nps.mv(d, -1, -3)

    # shape (Ndistances*Ny*Nx, 2)
    q = nps.clump(q, n=2)
    # shape (Ndistances*Ny*Nx, 3)
    p = nps.clump(p, n=2)

    # The list of distances. The meaning is the same as expected by
    # mrcal.show_projection_diff(): we visualize the diff of the FIRST distance
    distance_for_diff = d

    # Ignore any failed unprojections
    i_finite = np.isfinite(p[:,0])
    p = p[i_finite]
    q = q[i_finite]
    Npoints = len(q)
    weights = np.ones((Npoints,), dtype=float)

    ### Solve!

    ### I solve the optimization a number of times with different random seed
    ### values, taking the best-fitting results. This is required for the richer
    ### models such as LENSMODEL_OPENCV8
    err_rms_best               = 1e10
    intrinsics_data_best       = None
    rt_cam_ref_best = None

    for i in range(args.num_trials):
        # random seed for the new intrinsics
        intrinsics_core = intrinsics_from[1][:4]
        distortions     = (np.random.rand(Ndistortions) - 0.5) * 1e-3 # random initial seed
        intrinsics_to_values = nps.dummy(nps.glue(intrinsics_core, distortions, axis=-1),
                                         axis=-2)
        scale_down_rational_intrinsics(intrinsics_to_values)

        # each point has weight 1.0
        observations_points = nps.glue(q, nps.transpose(weights), axis=-1)
        observations_points = np.ascontiguousarray(observations_points) # must be contiguous. mrcal.optimize() should really be more lax here

        # Which points we're observing. This is dense and kinda silly for this
        # application. Each slice is (i_point,i_camera,i_camera-1). Initially O
        # do everything in camera-0 coordinates, and I do not move the
        # extrinsics
        indices_point_camintrinsics_camextrinsics = np.zeros((Npoints,3), dtype=np.int32)
        indices_point_camintrinsics_camextrinsics[:,0] = \
            np.arange(Npoints,    dtype=np.int32)
        indices_point_camintrinsics_camextrinsics[:,1] = 0
        indices_point_camintrinsics_camextrinsics[:,2] = -1

        optimization_inputs = \
            dict(intrinsics                                = intrinsics_to_values,
                 rt_cam_ref                                = None,
                 rt_ref_frame                              = None, # no frames. Just points
                 points                                    = p,
                 observations_board                        = None, # no board observations
                 indices_frame_camintrinsics_camextrinsics = None, # no board observations
                 observations_point                        = observations_points,
                 indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics,
                 lensmodel                                 = lensmodel_to,

                 imagersizes                               = nps.atleast_dims(dims, -2),

                 # I'm not optimizing the point positions (frames), so these
                 # need to be set to be inactive, and to include the ranges I do
                 # have
                 point_min_range                           = np.min(d) * 0.9,
                 point_max_range                           = np.max(d) * 1.1,

                 # I optimize the lens parameters. That's the whole point
                 do_optimize_intrinsics_core               = True,
                 do_optimize_intrinsics_distortions        = True,

                 do_optimize_extrinsics                    = False,

                 # NOT optimizing the observed point positions
                 do_optimize_frames                        = False )

        if re.match("LENSMODEL_SPLINED_STEREOGRAPHIC_", lensmodel_to):
            # splined models have a core, but those variables are largely redundant
            # with the spline parameters. So I lock down the core when targetting
            # splined models
            optimization_inputs['do_optimize_intrinsics_core'] = False

        stats = mrcal.optimize(**optimization_inputs,
                               # No outliers. I have the points that I have
                               do_apply_outlier_rejection        = False,
                               verbose                           = False)

        if not args.intrinsics_only:

            # go again, but refine this solution, allowing us to fit the
            # extrinsics too
            optimization_inputs['indices_point_camintrinsics_camextrinsics'][:,2] = \
                np.zeros ((Npoints,), dtype = np.int32)
            optimization_inputs['rt_cam_ref']  = (np.random.rand(1,6) - 0.5) * 1e-6
            optimization_inputs['do_optimize_extrinsics'] = True

            stats = mrcal.optimize(**optimization_inputs,
                                   # No outliers. I have the points that I have
                                   do_apply_outlier_rejection        = False,
                                   verbose                           = False)

        err_rms = stats['rms_reproj_error__pixels']
        print(f"RMS error of this solution: {err_rms} pixels.",
              file=sys.stderr)
        if err_rms < err_rms_best:
            err_rms_best = err_rms
            intrinsics_data_best  = optimization_inputs['intrinsics'][0,:].copy()
            if not args.intrinsics_only:
                rt_cam_ref_best = optimization_inputs['rt_cam_ref'][0,:].copy()

    if intrinsics_data_best is None:
        print("No valid intrinsics found!", file=sys.stderr)
        sys.exit(1)

    if args.num_trials > 1:
        print(f"RMS error of the BEST solution: {err_rms_best} pixels.",
              file=sys.stderr)

    m_to = mrcal.cameramodel( intrinsics = (lensmodel_to, intrinsics_data_best.ravel()),
                              imagersize = dims )

    if args.intrinsics_only:
        implied_Rt10 = mrcal.identity_Rt()
    else:
        implied_Rt10 = mrcal.Rt_from_rt(rt_cam_ref_best)

if not (args.sampled and args.intrinsics_only):
    implied_rt10 = mrcal.rt_from_Rt(implied_Rt10)
    print(f"Transformation cam_fitted <-- cam_input:  rotation: {nps.mag(implied_rt10[:3])*180./np.pi:.03f} degrees, translation: {np.array2string(implied_rt10[3:], precision=2)} m",
          file=sys.stderr)

    dist_shift = nps.mag(implied_rt10[3:])
    if dist_shift > 0.01:
        msg = f"## WARNING: fitted camera moved by {dist_shift:.03f}m. This is probably aphysically high, and something is wrong."
        if args.distance is not None and args.sampled:
            print(msg + " Pass both a high and a low --distance?",
                  file=sys.stderr)
        else:
            print(msg, file=sys.stderr)


note = \
    "generated on {} with   {}\n". \
    format(time.strftime("%Y-%m-%d %H:%M:%S"),
           ' '.join(mrcal.shellquote(s) for s in sys.argv))

if not args.all:

    m_to.Rt_cam_ref( mrcal.compose_Rt( implied_Rt10,
                                       mrcal.Rt_from_rt( m.rt_cam_ref())))

    if isinstance(file_output, str):
        f = file_output.replace('{CAMID}','')
        m_to.write(f, note=note)
        print(f"Wrote '{f}'", file=sys.stderr)
    else:
        # writing to stdout
        m_to.write(file_output, note=note)


else:
    # Write out the models for ALL the cameras. Today this assumes the vanilla
    # case of stationary cameras looking at a moving chessboard
    Ncameras = len(optimization_inputs['intrinsics'])
    for icam_intrinsics in range(Ncameras):
        m_to = mrcal.cameramodel( optimization_inputs = optimization_inputs,
                                  icam_intrinsics     = icam_intrinsics )
        if isinstance(file_output, str):
            f = file_output.replace('{CAMID}','-{:02d}'.format(icam_intrinsics))
            m_to.write(f, note=note)
            print(f"Wrote '{f}'", file=sys.stderr)
        else:
            # writing to stdout
            m_to.write(file_output, note=note)






if args.viz:

    plotkwargs_extra = {}
    if args.set is not None:
        plotkwargs_extra['set'] = args.set
    if args.unset is not None:
        plotkwargs_extra['unset'] = args.unset

    if args.title is not None:
        plotkwargs_extra['title'] = args.title
    if args.extratitle is not None:
        plotkwargs_extra['extratitle'] = args.extratitle

    # If this is a sampled solve, we don't have projection uncertainties. But we
    # do have an implied_Rt10 based on observations, and we can feed that to the
    # diff directly.
    #
    # If this is NOT a sampled solve, then the implied_Rt10 comes from a
    # Procrustes fit (minimizing error in 3D space), which is usually not
    # close-enough to get a diff. We let the diff method figure out the
    # implied_Rt10, and we use the uncertainties that we have to do it
    if args.sampled:
        use_uncertainties = False
    else:
        implied_Rt10      = None
        use_uncertainties = True

    plot,_ = mrcal.show_projection_diff( (m, m_to),
                                         implied_Rt10      = implied_Rt10,
                                         distance          = distance_for_diff,
                                         cbmax             = args.cbmax,
                                         gridn_width       = None if args.gridn is None else args.gridn[0],
                                         gridn_height      = None if args.gridn is None else args.gridn[1],
                                         use_uncertainties = use_uncertainties,
                                         hardcopy          = args.hardcopy,
                                         terminal          = args.terminal,
                                         **plotkwargs_extra)
    if args.hardcopy is None:
        plot.wait()