mrcal-pick-features

#!/usr/bin/python3

r'''xxx'''

import sys
import argparse
import re
import os

def parse_args():

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


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

    parser.add_argument('--clahe',
                        action='store_true',
                        help='''If given, apply CLAHE equalization to the images
                        prior to the stereo matching''')

    ######## stereo processing
    parser.add_argument('--valid-intrinsics-region',
                        action='store_true',
                        help='''If given, annotate the image with its
                        valid-intrinsics region''')
    parser.add_argument('--range-nominal',
                        type=float,
                        default=10.,
                        help='''Initial guess for the range of all picked
                        points. Defaults to 10m''')
    parser.add_argument('--template-size',
                        type=positive_int,
                        nargs=2,
                        default = (13,13),
                        help='''The size of the template used for feature
                        matching, in pixel coordinates of the second image. Two
                        arguments are required: width height. This is passed
                        directly to mrcal.match_feature(). We default to
                        13x13''')
    parser.add_argument('--search-radius',
                        type=positive_int,
                        default = 20,
                        help='''How far the feature-matching routine should
                        search, in pixel coordinates of the second image. This
                        should be larger if the nominal range estimate is poor,
                        especially, at near ranges. This is passed directly to
                        mrcal.match_feature(). We default to 20 pixels''')
    parser.add_argument('--single-buffered',
                        action='store_true',
                        help='''By default the image display is double-buffered
                        to avoid flickering. Some graphics hardare (in
                        particular my i915-based laptop) doesn't work right in
                        this mode, so --single-buffered is available to disable
                        double-buffering as a work around''')

    parser.add_argument('models',
                        type=str,
                        nargs = 2,
                        help='''Camera models representing cameras used to
                        capture the images. Intrinsics only are used. A nominal
                        stereo geometry with unit baseline is assumed. Given
                        models are the left, right cameras''')
    parser.add_argument('images',
                        type=str,
                        nargs=2,
                        help='''The images to use for the matching''')

    args = parser.parse_args()

    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




from fltk               import *
from Fl_Gl_Image_Widget import *

import numpy as np
import numpysane as nps
import mrcal
import pyopengv

def load_image(path):
    try:
        image = mrcal.load_image(path)
    except:
        print(f"Couldn't read image '{path}'", file=sys.stderr)
        sys.exit(1)

    # I want to intelligently handle the possible image formats. Specifically
    # the 16-bit case needs special care

    # color images: use the mean of the channels to convert to grayscale
    if image.ndim == 3:
        image = \
            np.mean(image,
                    axis  = -1,
                    dtype = image.dtype)
    elif image.ndim != 2:
        raise Exception(f"Image '{path}': only ndim==2 (monochrome) or ndim==3 (color) are supported")

    # Deep images: stretch equalization
    if image.itemsize > 1:
        if image.dtype != np.uint16:
            raise Exception("Only 16-bit deep images are implemented currently; I assume this in the following code")
        # I apply a stretch equalization, and downsample to 8-bits
        amin = np.min(image)
        amax = np.max(image)
        d = amax-amin
        if d == 0:
            raise Exception(f"Image '{path}' has the value '{amax}' on every pixel. This can't match any features")

        # I want to compute
        #   (image - amin) / (amax-amin) * 255
        image = ((image - amin).astype(float) / d * 255.).astype(np.uint8)

    return image


def get_q01_nominal(*,
                    q,
                    index):
    i_from     = index
    i_to       = 1-index


    if index == 0:
        Rt_to_from = \
            mrcal.compose_Rt( models[1].extrinsics_Rt_fromref(),
                              models[0].extrinsics_Rt_toref() )
    else:
        Rt_to_from = \
            mrcal.compose_Rt( models[0].extrinsics_Rt_fromref(),
                              models[1].extrinsics_Rt_toref() )

    v_from = mrcal.unproject(q, *models[i_from].intrinsics(),
                        normalize = True)
    p_from = v_from * args.range_nominal
    p_to = mrcal.transform_point_Rt(Rt_to_from, p_from)
    q_to = mrcal.project(p_to, *models[i_to].intrinsics())

    if index == 0:
        return nps.cat(q, q_to)
    else:
        return nps.cat(q_to, q)


def solve_R01__kneip(v0,v1):
    r'''Rotation from corresponding-feature observations: Kneip's eigesolver

Kneip describes a method to compute the rotation:

  L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
  Images Independently of the Translation", Proc. of The European Conference on
  Computer Vision (ECCV), Florence, Italy. October 2012.

  L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
  The International Conference on Computer Vision (ICCV), Sydney, Australia.
  December 2013.
'''
    return \
        pyopengv.relative_pose_eigensolver(v0,v1,
                                           # seed
                                           mrcal.identity_R())


def solve_R01__optimize(v0,v1):
    r'''Rotation from corresponding-feature observations: direct optimization

This is for experimenting. solve_R01__kneip() should work well.
'''
    import scipy.optimize

    def cost(r01):
        n = np.cross(v0,
                     mrcal.rotate_point_r(r01,v1))
        l,v = mrcal.sorted_eig(nps.matmult(n.T,n))
        return l[0]

    res = \
        scipy.optimize.minimize( cost,
                                 x0 = mrcal.identity_r(),
                                )
    r01 = res.x
    return mrcal.R_from_r(r01)


def solve_Rt01_given_R01__geometric(v0,v1,R01):
    r'''Translation from corresponding-feature observations: Dima's method

Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.

This method assumes a rotation is already available. Given this rotation this
method computes the translation. This may or may not be any better than
solve_Rt01_given_R01__epipolar_plane_normality(). That needs evaluation.

The derivation of this method follows. I use the geometric triangulation
expression derived here:

  https://github.com/dkogan/mrcal/blob/8be76fc28278f8396c0d3b07dcaada2928f1aae0/triangulation.cc#L112

I assume that I'm triangulating normalized v0,v1 both expressed in cam-0
coordinates. And I have a t01 translation that I call "t" from here on. This is
unique only up-to-scale, so I assume that norm2(t) = 1. The geometric
triangulation from the above link says that

  [k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1   v0.v1][ t01.v0]
  [k1]                               [ v0.v1   v0.v0][-t01.v1]

  The midpoint p is

  p = (k0 v0 + t01 + k1 v1)/2

I assume that v are normalized and I represent k as a vector. I also define

  c = inner(v0,v1)

So

  k = 1/(1 - c^2) [1 c] [ v0t] t
                  [c 1] [-v1t]

I define

  A = 1/(1 - c^2) [1 c]     This is a 2x2 array
                  [c 1]

  B = [ v0t]                This is a 2x3 array
      [-v1t]

  V = [v0 v1]               This is a 3x2 array

Note that none of A,B,V depend on t.

So

  k = A B t

Then

  p = (k0 v0 + t01 + k1 v1)/2
    = (V k + t)/2
    = (V A B t + t)/2
    = (I + V A B) t/2

Each triangulated error is

  e = mag(p - k0 v0)

I split A into its rows

  A = [ a0t ]
      [ a1t ]

Then

  e = p - k0 v0
    = (I + V A B) t/2 - v0 a0t B t
    = I t/2 + V A B t/2 - v0 a0t B t
    = I t/2 + v0 a0t B t/2 + v1 a1t B t/2 - v0 a0t B t
    = I t/2 - v0 a0t B t/2 + v1 a1t B t/2
    = ((I - v0 a0t B + v1 a1t B) t) / 2
    = ((I + (- v0 a0t + v1 a1t) B) t) / 2
    = ((I - Bt A B) t) / 2

I define a joint error function I'm optimizing as the sum of all the individual
triangulation errors:

  E = sum(norm2(e_i))

Each component is

  norm2(e) = 1/4 tt (I - Bt A B)t (I - Bt A B) t
           = 1/4 tt (I - 2 Bt A B + Bt A B Bt A B ) t

  B Bt = [1  -c]
         [-c  1]

  B Bt A = 1/(1 - c^2) [1  -c] [1 c]
                       [-c  1] [c 1]
         = 1/(1 - c^2) [1-c^2  0     ]
                       [0      1-c^2 ]
         = I

-> norm2(e) = 1/4 tt (I - 2 Bt A B + Bt A B) t
            = 1/4 tt (I - Bt A B) t
            = 1/4 - 1/4 tt Bt A B t

So

    E = N/4 - 1/4 tt sum(Bt A B) t

I let

    M  = sum(Bt A B)
    M  = sum(Mi)
    Mi = Bt A B

So

    E = N/4 - 1/4 tt M t
      = N/4 - 1/4 lambda

So to minimize E I find t that is the eigenvector of M that corresponds to its
largest eigenvalue lambda. Furthermore, lambda depends on the rotation. If I
couldn't estimate the rotation from far-away features I can solve the
eigenvalue-optimization problem to maximize lambda.

More simplification:

    Mi = Bt A B = [ v0  -v1 ] A [ v0t]
                                [-v1t]
       = 1/(1-c^2) [ v0 - v1 c    v0 c - v1] [ v0t]
                                             [-v1t]
       = 1/(1-c^2) ((v0 - v1 c) v0t - (v0 c - v1) v1t)

    c  = v0t v1 ->
    F0 = v0 v0t
    F1 = v1 v1t

    -> Mi = 1/(1-c^2) (v0 v0t - v1 v1t v0 v0t + v1 v1t - v0 v0t v1 v1t)
          = 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
          = (F0 - F1)^2 / (1 - c^2)
          = ((F0 - F1)/s)^2

    where s = mag(cross(v0,v1))


    tt M t = sum( norm2((F0i - F1i)/si t) )

    Let Di = (F0i - F1i)/si

    I want to maximize sum( norm2(Di t) )


    (F0 - F1)/s = (v0 v0t - v1 v1t) / mag(cross(v0,v1))
                ~ (v0 v0t - R v1 v1t Rt) / mag(cross(v0,R v1))

experiments:

          = 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
          = 1/(1-c^2) (v0 v0t + v1 v1t - (c v0 v1t + c v1 v0t))

    '''
    v1 = mrcal.rotate_point_R(R01,v1)

    # shape (N,)
    c = nps.inner(v0,v1)

    N = len(c)

    # shape (N,2,2)
    A = np.ones((N,2,2), dtype=float)
    A[:,0,1] = c
    A[:,1,0] = c
    A /= nps.mv(1. - c*c, -1,-3)

    # shape (N,2,3)
    B = np.empty((N,2,3), dtype=float)
    B[:,0,:] =  v0
    B[:,1,:] = -v1

    # shape (3,3)
    M = np.sum( nps.matmult(nps.transpose(B), A, B), axis = 0 )

    l,v = mrcal.sorted_eig(M)

    # The answer is the eigenvector corresponding to the biggest eigenvalue
    t01 = v[:,2]

    # Almost done. I want either t or -t. The wrong one will produce
    # mostly triangulations behind me
    k = nps.matmult(A,B, nps.transpose(t01))[..., 0]

    mask_divergent_t    = (k[:,0] <= 0) + (k[:,1] <= 0)
    mask_divergent_negt = (k[:,0] >= 0) + (k[:,1] >= 0)
    N_divergent_t    = np.count_nonzero( mask_divergent_t )
    N_divergent_negt = np.count_nonzero( mask_divergent_negt )

    if N_divergent_t == 0 or N_divergent_negt == 0:

        # from before: norm2(e) = 1/4 - 1/4 tt Bt A B t
        # shape (N,2,1)
        Bt = nps.dummy(nps.inner(B, t01),
                       -1)

        # shape (N,1,1)
        tBtABt = nps.matmult(nps.transpose(Bt), A, Bt)

        # shape (N,)
        tBtABt = tBtABt[:,0,0]

        norm2e = 1/4 * (1 - tBtABt)

    else:
        norm2e = None

    if N_divergent_t < N_divergent_negt:
        return ( nps.glue(R01, t01, axis=-2),
                 mask_divergent_t,
                 N_divergent_t,
                 norm2e )
    else:
        return ( nps.glue(R01, -t01, axis=-2),
                 mask_divergent_negt,
                 N_divergent_negt,
                 norm2e )


def solve_Rt01_given_R01__epipolar_plane_normality(v0,v1,R01):
    r'''Translation from corresponding-feature observations: Kneip's method

Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.

This method assumes a rotation is already available (from Kneip's eigensolver,
for instance). Given this rotation this method computes the translation. This
may or may not be any better than solve_Rt01_given_R01__geometric(). That needs
evaluation.

Kneip describes a method to compute the ROTATION:

  L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
  Images Independently of the Translation", Proc. of The European Conference on
  Computer Vision (ECCV), Florence, Italy. October 2012.

  L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
  The International Conference on Computer Vision (ICCV), Sydney, Australia.
  December 2013.

The rotation is the hard part, but we do still need a translation. opengv should
do this for us, and I think its C++ API does, but its Python bindings are
lacking. So I compute t myself for now

    '''

    Rt01 = np.zeros((4,3), dtype=float)
    Rt01[:3,:] = R01

    # shape (N,3)
    c = np.cross(v0, mrcal.rotate_point_R(R01, v1))
    l,v = mrcal.sorted_eig(np.sum(nps.outer(c,c), axis=0))
    # t is the eigenvector corresponding to the smallest eigenvalue
    t01 = v[:,0]

    Rt01[3,:] = t01

    # Almost done. I want either t or -t. The wrong one will produce
    # mostly triangulations behind me
    p_t = mrcal.triangulate_geometric(v0, v1,
                                      v_are_local = True,
                                      Rt01        = Rt01 )
    mask_divergent_t = (nps.norm2(p_t) == 0)
    N_divergent_t    = np.count_nonzero( mask_divergent_t )

    Rt01_negt = Rt01 * nps.transpose(np.array((1,1,1,-1),))
    p_negt = mrcal.triangulate_geometric(v0, v1,
                                         v_are_local = True,
                                         Rt01        = Rt01_negt )
    mask_divergent_negt = (nps.norm2(p_negt) == 0)
    N_divergent_negt    = np.count_nonzero( mask_divergent_negt )

    if N_divergent_t == 0 and N_divergent_negt == 0:
        raise Exception("Cannot ambiguously determine t: neither has divergent observations")

    # We have divergences even in the best case. I pick the fewer-divergence
    # case, and report the diverging observations as outliers. All converging
    # observations are treated as inlier here, regardless of reprojection error
    if N_divergent_t < N_divergent_negt:
        return (Rt01,
                mask_divergent_t,
                N_divergent_t,
                None)
    else:
        return (Rt01_negt,
                mask_divergent_negt,
                N_divergent_negt,
                None)


def solve_Rt10(# shape (N,Nleftright=2,Nxy=2)
               q01,
               intrinsics0,
               intrinsics1,
               *,
               solve_R10           = solve_R01__kneip,
               solve_Rt10_from_R01 = solve_Rt01_given_R01__geometric):
    r'''Full-transform optimization

Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.

The methods to compute the rotation and the translation are given in solve_R10
and solve_Rt10_from_R01.

    '''

    # shape (N,3)
    # These are in their LOCAL coord system
    v0 = mrcal.unproject(q01[...,0,:], *intrinsics0, normalize = True)
    v1 = mrcal.unproject(q01[...,1,:], *intrinsics1, normalize = True)


    # Keep all all points initially
    mask_inliers = np.ones( (q01.shape[0],), dtype=bool )

    while True:

        if np.count_nonzero(mask_inliers) == 0:
            raise Exception("All points thrown out as outliers")

        R01 = solve_R10(v0[mask_inliers],
                        v1[mask_inliers])

        Rt01, mask_outlier, Noutliers, norm2e = \
            solve_Rt10_from_R01(v0[mask_inliers],
                                v1[mask_inliers],
                                R01 = R01)
        if Noutliers == 0:
            break

        mask_inliers[np.nonzero(mask_inliers)[0][mask_outlier]] = False

    return \
        Rt01, \
        mask_inliers

def update_p0_triangulated_stored():
    global p0_triangulated_stored
    Rt01 = mrcal.compose_Rt( models[0].extrinsics_Rt_fromref(),
                             models[1].extrinsics_Rt_toref() )


    q0 = q01_stored[...,0,:]
    q1 = q01_stored[...,1,:]
    v0 = mrcal.unproject(q0, *models[0].intrinsics(), normalize = True)
    v1 = mrcal.unproject(q1, *models[1].intrinsics(), normalize = True)
    p0_triangulated_stored = \
        mrcal.triangulate_geometric(v0, v1,
                                    v_are_local = True,
                                    Rt01        = Rt01 )

def update_q01_stored(_q01_stored):
    global q01_stored

    q01_stored = _q01_stored
    update_p0_triangulated_stored()


def line_segments_squares(# shape (..., 2)
                          q,
                          radii):

    Nradii = len(radii)

    # shape (..., Nradii, Nsegments_in_square=4, Npoints_in_line_segment=2, xy=2)
    p = np.zeros(q.shape[:-1] + (Nradii,4,2,2), dtype=np.float32)
    p += nps.dummy(q,-2,-2,-2)

    rx = np.array((1,0), dtype=np.float32)
    ry = np.array((0,1), dtype=np.float32)

    for i,radius in enumerate(radii):

        # line segment 0
        p[..., i,0,0,:] += (-rx-ry)*radius
        p[..., i,0,1,:] += (-rx+ry)*radius
        # line segment 1
        p[..., i,1,0,:] += (+rx-ry)*radius
        p[..., i,1,1,:] += (+rx+ry)*radius
        # line segment 2
        p[..., i,2,0,:] += (-rx-ry)*radius
        p[..., i,2,1,:] += (+rx-ry)*radius
        # line segment 3
        p[..., i,3,0,:] += (-rx+ry)*radius
        p[..., i,3,1,:] += (+rx+ry)*radius

    # flatten to a list of line segments
    # shape (N, 2,2)
    return nps.clump(p, n=p.ndim-2)

def line_segments_crosshairs(# shape (..., 2)
                             q,
                             radius):

    # shape (..., Nsegments_in_crosshair=2, Npoints_in_line_segment=2, xy=2)
    p = np.zeros(q.shape[:-1] + (4,2,2), dtype=np.float32)
    p += nps.dummy(q,-2,-2)

    rx = np.array((1,0), dtype=np.float32)
    ry = np.array((0,1), dtype=np.float32)

    # line segment 0
    p[..., 0,0,:] += (-rx-ry)*radius
    p[..., 0,1,:] += (+rx+ry)*radius
    # line segment 1
    p[..., 1,0,:] += (+rx-ry)*radius
    p[..., 1,1,:] += (-rx+ry)*radius

    # flatten to a list of line segments
    # shape (..., 2,2)
    return nps.clump(p, n=p.ndim-2)


def set_all_overlay_lines_and_redraw():

    point_radius_stored = 10
    crosshair_radius    = 1

    point_color_stored  = np.array((0,1,0), dtype=np.float32)

    point_radius_selected = 11
    point_color_selected  = np.array((0,0,1), dtype=np.float32)

    Nstored = len(q01_stored)

    Rt01 = mrcal.compose_Rt( models[0].extrinsics_Rt_fromref(),
                             models[1].extrinsics_Rt_toref() )
    baseline = nps.mag(Rt01[3,:])


    # for projected rays, I start at a small ratio of the baseline, and increase
    # the range geometrically. I add another point at infinity at the end
    k = 1.1
    N = 25
    r = baseline * 0.5 * (k ** np.arange(N))

    iselected = tuple(i for i in range(Nstored) if widget_table.row_selected(i))

    for w in widgets_image:

        lines_stored = \
            dict(points    = nps.glue( line_segments_squares(q01_stored[:, w.index,:],
                                                             (point_radius_stored,)),
                                       line_segments_crosshairs(q01_stored[:, w.index,:],
                                                                crosshair_radius),
                                       axis = -3 ),
                 color_rgb = point_color_stored )

        if len(iselected):

            # shape (Nselected, 2)
            q_stored       = q01_stored[iselected,   w.index,:]
            q_stored_other = q01_stored[iselected, 1-w.index,:]

            # shape (Nranges, Nselected, 3)
            p = mrcal.unproject(q_stored_other, *models[1-w.index].intrinsics(),
                                normalize = True) * \
                    nps.mv(r,-1,-3)
            if w.index == 0:
                pinf = mrcal.rotate_point_R(Rt01[:3,:], p[0])
                p    = mrcal.transform_point_Rt(Rt01, p)
            else:
                pinf = mrcal.rotate_point_R(mrcal.invert_R(Rt01[:3,:]), p[0])
                p    = mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01), p)

            # shape (Nsegments, Nsegmentpoint=2, Nxy=2)
            qsegments = np.zeros( (0,2,2), dtype=np.float32)

            # shape (Nranges+1, Nselected, Nxyz=3)
            p = nps.glue(p,pinf, axis=-3)

            for pselected in nps.mv(p,-2,-3):
                # pselected.shape is (Nranges+1,Nxyz=3)

                # cut off points where the ray is behind the other camera
                pselected = pselected[pselected[:,2] > 0,:]

                if len(pselected)>=2:
                    # shape (N, Nxy=2)
                    q = mrcal.project(pselected,
                                      *models[w.index].intrinsics()).astype(np.float32)
                    # shape (N-1, Nsegmentpoint=2, Nxy=2)
                    qsegments = \
                        nps.glue(qsegments,
                                 nps.mv( nps.cat(q[0:-1,...],q[1:,...]),
                                         0,1 ),
                                 axis=-3)

            w.set_lines( lines_stored,
                         dict(points    = nps.glue( line_segments_squares(q_stored,
                                                                          (point_radius_selected,),),
                                                    qsegments,
                                                    axis = -3),
                                  color_rgb = point_color_selected ) )
        else:
            w.set_lines(lines_stored)


def widget_table_callback(*args):
    ctx   = widget_table.callback_context()
    event = Fl.event()
    if ctx == Fl_Table.CONTEXT_CELL and \
       event == FL_RELEASE:
        set_all_overlay_lines_and_redraw()

def widget_solve_callback(*args):
    try:
        Rt01, mask_inliers = \
            solve_Rt10(q01_stored,
                       *[m.intrinsics() for m in models],
                       solve_Rt10_from_R01 = solve_Rt01_given_R01__geometric)
    except Exception as e:
        widget_info.value(UI_usage_message + "\n" + \
                          f"Solve failed: {e}")
        return

    Rtr0 = models[0].extrinsics_Rt_toref()
    Rtr1 = mrcal.compose_Rt(Rtr0, Rt01)
    models[1].extrinsics_Rt_toref(Rtr1)

    update_p0_triangulated_stored()

    widget_table.redraw()
    set_all_overlay_lines_and_redraw()

    N = len(q01_stored)
    Noutliers = N-np.count_nonzero(mask_inliers)
    widget_info.value(UI_usage_message + "\n" + \
                      f"Extrinsics of model1 updated: {Noutliers}/{N} outliers")


def widget_write_callback(*args):
    np.savetxt(sys.stdout,
               nps.clump(q01_stored[:,:4],
                         n = -2),
               fmt = '%.2f',
               header = "x0 y0 x1 y1")


class Fl_Gl_Image_Widget_Derived(Fl_Gl_Image_Widget):

    def __init__(self,
                 *args,
                 index,
                 **kwargs):
        self.index = index
        return super().__init__(*args, **kwargs)

    def set_panzoom(self,
                    x_centerpixel, y_centerpixel,
                    visible_width_pixels,
                    notify_other_widgets = True):
        r'''Pan/zoom the image

        This is an override of the function to do this: any request to
        pan/zoom the widget will come here first. I dispatch any
        pan/zoom commands to all the widgets, so that they all work in
        unison. visible_width_pixels < 0 means: this is the redirected
        call; just call the base class

        '''
        if not notify_other_widgets:
            return super().set_panzoom(x_centerpixel, y_centerpixel,
                                       visible_width_pixels)

        # All the widgets should pan/zoom together
        result = \
            all( w.set_panzoom(x_centerpixel, y_centerpixel,
                               visible_width_pixels,
                               notify_other_widgets = False) \
                 for w in widgets_image )

        # Switch back to THIS widget
        self.make_current()
        return result

    def handle_right_mouse_button(self):
        try:
            q = \
                np.array( self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), ),
                          dtype=float )
        except:
            widget_info.value(UI_usage_message + "\n" + \
                              "Error converting pixel coordinates")
            return 1

        if not (q[0] >= -0.5 and q[0] <= W-0.5 and \
                q[1] >= -0.5 and q[1] <= H-0.5):
            widget_info.value(UI_usage_message + "\n" + \
                              f"Out of bounds: {q=}")
            return 1

        # shape (2,2): (leftright, qxy)
        q01 = get_q01_nominal(q     = q,
                              index = self.index)

        try:
            if self.index == 0:
                match_feature_out = \
                    mrcal.match_feature(images[0], images[1],
                                        q0               = q01[0],
                                        q1_estimate      = q01[1],
                                        search_radius1   = args.search_radius,
                                        template_size1   = args.template_size)
            else:
                match_feature_out = \
                    mrcal.match_feature(images[1], images[0],
                                        q0               = q01[1],
                                        q1_estimate      = q01[0],
                                        search_radius1   = args.search_radius,
                                        template_size1   = args.template_size)
            q_other, match_feature_diagnostics = match_feature_out[:2]
        except:
            q_other = None

        if q_other is None:
            widget_info.value(UI_usage_message + "\n" + \
                              "Error matching feature")
            return 1


        q01[1-self.index] = q_other

        update_q01_stored(nps.glue( q01_stored,
                                    q01,
                                    axis=-3))

        Nstored = len(q01_stored)
        widget_table.rows( Nstored )
        widget_table.select_all_rows(0) # deselect all
        widget_table.select_row(Nstored-1)
        set_all_overlay_lines_and_redraw()

        widget_info.value(UI_usage_message + "\n" + \
                          "Feature match successful")

        return 1

    def handle(self, event):
        if event == FL_ENTER:
            return 1
        if event == FL_LEAVE:
            widget_status.value("")
            return 1
        if event == FL_MOVE:
            try:
                q = self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), )
                this = f"Image {self.index}"
                widget_status.value(f"{this}: {q[0]:.2f},{q[1]:.2f}")
            except:
                widget_status.value("")
            return 1

        if event == FL_PUSH and Fl.event_button() == FL_LEFT_MOUSE:
            self.dragged = False
        if event == FL_DRAG and Fl.event_state() & FL_BUTTON1:
            self.dragged = True
        # both of these fall through to call the parent

        if event == FL_RELEASE:
            if Fl.event_button() == FL_RIGHT_MOUSE:
                return self.handle_right_mouse_button()
            if Fl.event_button() == FL_LEFT_MOUSE:
                if not self.dragged:
                    # Clicked. Select the nearest feature
                    if len(q01_stored) > 0:
                        qwidget = np.array((Fl.event_x(),Fl.event_y()),
                                           dtype=int)
                        qwidget_stored = \
                            np.array([self.map_pixel_viewport_from_image(q) for q in q01_stored[:,self.index,:]])
                        # within 10 pixels in the UI
                        i = np.nonzero(nps.norm2(qwidget - qwidget_stored) < 10*10)[0]
                        if len(i) < 1:
                            widget_info.value(UI_usage_message + "\n" + \
                                              "No feature near click")
                            widget_table.select_all_rows(0) # deselect all
                            set_all_overlay_lines_and_redraw()
                        elif len(i) > 1:
                            widget_info.value(UI_usage_message + "\n" + \
                                              "Too many features near click")
                            widget_table.select_all_rows(0) # deselect all
                            set_all_overlay_lines_and_redraw()
                        else:
                            # Just one feature. Select it
                            i = i[0]
                            widget_table.select_all_rows(0) # deselect all
                            widget_table.select_row(int(i))
                            set_all_overlay_lines_and_redraw()
        # fall through to call the parent

        # Must be key UP, not key DOWN. Because of https://github.com/fltk/fltk/issues/1044
        if event == FL_KEYUP:
            if Fl.event_key() == fltk.FL_Delete:
                i_keep = tuple(i for i in range(widget_table.rows()) \
                               if not widget_table.row_selected(i))
                update_q01_stored(q01_stored[i_keep, ...])
                widget_table.rows(len(q01_stored))
                widget_table.select_all_rows(0) # deselect all
                set_all_overlay_lines_and_redraw()
                widget_info.value(UI_usage_message + "\n" + \
                                  "Feature(s) deleted")

                return 1

        return super().handle(event)


class Fl_Table_Derived(Fl_Table_Row):

    def __init__(self, x, y, w, h, *args):
        Fl_Table_Row.__init__(self, x, y, w, h, *args)

        self.col_labels = \
            [ "x0",
              "y0",
              "x1",
              "y1",
              "Triangulated range",
              "Cam0 triangulated reprojection error", ]

        self.type(fltk.Fl_Table_Row.SELECT_MULTI)
        self.rows(len(q01_stored))
        self.cols(len(self.col_labels))
        self.col_header(1)
        self.col_resize(1)

        self.when(FL_WHEN_RELEASE)
        self.callback(widget_table_callback)

        self.end()

    def draw_cell(self, context, row, col, x, y, w, h):

        if context == self.CONTEXT_STARTPAGE:
            fl_font(FL_HELVETICA, 12)
            return

        if context == self.CONTEXT_COL_HEADER:
            text = self.col_labels[col]

            fl_push_clip(x, y, w, h)
            fl_draw_box(FL_THIN_UP_BOX, x, y, w, h, self.row_header_color())
            fl_color(FL_BLACK)
            fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)
            fl_pop_clip()
            return

        if context == self.CONTEXT_CELL:
            if col < 4:
                iimage = col // 2
                ixy    = col %  2
                text = f"{q01_stored[row,iimage,ixy]:.2f}"
            else:
                p = p0_triangulated_stored[row]
                if nps.norm2(p) == 0:
                    # Divergent feature. No triangulated anything available
                    text = '-'
                else:
                    if col == 4:
                        # range
                        text = f"{nps.mag(p):.2f}"
                    else:
                        # reprojection error
                        icam = 0
                        text = f"{nps.mag(q01_stored[row,icam] - mrcal.project(p, *models[icam].intrinsics())):.1f}"



            fl_push_clip(x, y, w, h)
            # background color
            fl_color(self.selection_color() if self.row_selected(row) else FL_WHITE)
            fl_rectf(x, y, w, h)

            # text
            fl_color(FL_BLACK)
            fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)

            # border
            fl_color(FL_LIGHT2)
            fl_rect(x, y, w, h)
            fl_pop_clip()

            return

        return



models = [mrcal.cameramodel(m) for m in args.models]

models[0].extrinsics_rt_toref(np.array((0,0,0, 0,0,0), dtype=float))
models[1].extrinsics_rt_toref(np.array((0,0,0,-1,0,0), dtype=float))

W,H = models[0].imagersize()

if args.clahe:
    # importing cv2 is slow, so I only do it if necessary
    import cv2
    clahe = cv2.createCLAHE()
    clahe.setClipLimit(8)

images = [load_image(f) for f in args.images]

if args.clahe:
    images = [ clahe.apply(image) for image in images ]

if args.valid_intrinsics_region:
    for i in range(2):
        mrcal.annotate_image__valid_intrinsics_region(images[i], models[i])


UI_usage_message = r'''Usage:

Left mouse button click/drag: pan
Mouse wheel up/down/left/right: pan
Ctrl-mouse wheel up/down: zoom
'u': reset view: zoom out, pan to the center

Right mouse button click: Find matching feature
Click in table: select feature(s)
Delete: delete selected feature(s)
'''



## these two arrays correspond to each other. If you update one, do update the
## other
# shape (N, Nimages = 2,xy=2)
q01_stored = np.zeros((0,2,2), dtype=float)
# shape (N, xy=2)
p0_triangulated_stored = np.zeros((0,2), dtype=float)






WINDOW_W = 800
IMAGES_H = 300
TABLE_H  = 300
BUTTON_H = 30
STATUS_H = 20

WINDOW_H = IMAGES_H + TABLE_H + BUTTON_H + STATUS_H

window = Fl_Window(WINDOW_W, WINDOW_H, "mrcal feature picker")
body   = Fl_Group(0,0,
                  WINDOW_W, IMAGES_H + TABLE_H)

y = 0
widgets_image = (Fl_Gl_Image_Widget_Derived(0,    y,
                                            WINDOW_W//2,IMAGES_H,
                                            index = 0,
                                            double_buffered = not args.single_buffered),
                 Fl_Gl_Image_Widget_Derived(WINDOW_W//2,  y,
                                            WINDOW_W//2,IMAGES_H,
                                            index = 1,
                                            double_buffered = not args.single_buffered))
y += IMAGES_H


widget_table  = Fl_Table_Derived(   0,  y,
                                    WINDOW_W//2,TABLE_H)
widget_info   = Fl_Multiline_Output(WINDOW_W//2,y,
                                    WINDOW_W//2,TABLE_H)
y += TABLE_H
body.end()

Nbuttons = 2
BUTTON_W = WINDOW_W//Nbuttons
ibutton = 0

widget_solve_button = Fl_Button(ibutton*BUTTON_W, y,
                                BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
                                BUTTON_H, "Solve for the extrinsics")
widget_solve_button.callback(widget_solve_callback)
ibutton+=1

widget_write_button = Fl_Button(ibutton*BUTTON_W, y,
                                BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
                                BUTTON_H, "Write output to stdout")
widget_write_button.callback(widget_write_callback)
ibutton+=1

y += BUTTON_H

widget_status = Fl_Output(0,y,
                          WINDOW_W,STATUS_H)
widget_info.value(UI_usage_message)
y += STATUS_H


window.resizable(body)
window.end()
window.show()

for i in range(2):
    widgets_image[i]. \
      update_image(decimation_level = 0,
                   image_data       = images[i])

if len(q01_stored):
    # This requires gl-image-display 0.19, so I only do it if I need to. Most of
    # the time we will start out with no features, so older gl-image-display
    # would be fine
    set_all_overlay_lines_and_redraw()

Fl.run()

sys.exit(0)





r'''

'u' should pan to the center

ctrl-drag should lock panning

arrow-keys in the table should work

better UI messages
'''