focus

#!/usr/bin/env python3

r'''Report image sharpness over time

SYNOPSIS

  $ ./focus

  [ Interactive window up pop up showing the realtime view; clicking on the ]
  [ image shows a realtime plot of the sharpness in those areas             ]

This tool is intended to automate camera-focussing procedures. It repeatedly
grabs images, and plots a "sharpness" metric in each region of interest. The
user moves the focus ring, while looking at the realtime feedback displayed by
this tool.

'''


import sys
import argparse
import re
import os

import mrcam

def parse_args():

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

    parser.add_argument('--radius',
                        type=int,
                        default = 50,
                        help='''The radius of the boxes that define the focusing
                        region-of-interest. If omitted, defaults to 50 pixels''')
    parser.add_argument('--plot-history',
                        type=float,
                        default = 30,
                        help='''The width of the sharpness plot. If omitted,
                        defaults to 30 seconds''')

    mrcam._add_common_cmd_options(parser,
                                  single_camera = True)

    args = parser.parse_args()

    mrcam._parse_args_postprocess(args)

    return args


args = parse_args()

import time
import numpy as np
import numpysane as nps
import cv2
from fltk import *
from Fl_gnuplotlib import *


focus_regions = []
imagersize = None
t0 = time.time()





# from "gnuplot -e 'show linetype'"
color_sequence_rgb = (
    "0x9400d3",
    "0x009e73",
    "0x56b4e9",
    "0xe69f00",
    "0xf0e442",
    "0x0072b2",
    "0xe51e10",
    "0x000000"
)

def color_array_from_string(c):
    x = int(c, 0)

    r = (x >> 16) & 255
    g = (x >>  8) & 255
    b = (x >>  0) & 255
    return np.array((r,g,b), dtype=np.float32) / 255.

def sharpness(image):
    '''Reports a measure of sharpness in an image.

    There're many ways to do this. I look at this:

        Said Pertuz, Domenec Puig, Miguel Angel Garcia, "Analysis of focus
        measure operators for shape-from-focus", Pattern Recognition Volume 46,
        Isue 5, May 2013.

    They looked at a bunch of operators, and there wasn't a standout. I use the
    basic norm2(laplacian) since it's simple, and they had decent results with
    it. This is LAP1 described in section A.17.

    '''

    # image may be high-depth and it may be in color
    if image.ndim == 3:
        image = nps.mag(image, dtype=np.float32)
    elif image.ndim == 2:
        image = image.astype(np.float32)
    else:
        raise Exception(f"image.shape must be 2 or 3; dont't know what to do with {image.shape=}")

    L = cv2.Laplacian(image,
                      ddepth = cv2.CV_32F,
                      borderType = cv2.BORDER_REFLECT)
    return nps.norm2(L.ravel())

def update_sharpness(image,
                     **kwargs_extra):
    global imagersize

    if image is None:
        # Error occurred capturing the image. Let it go; hopefully the next one
        # will work
        return

    t = time.time()-t0

    if imagersize is None:
        imagersize = np.array((image.shape[1], image.shape[0]),)

    for region in focus_regions:
        roi  = region['roi']
        region['data'] = nps.glue(region['data'],
                                  np.array((t, sharpness(image[roi[0],roi[1]])),
                                           dtype=float),
                                  axis = -2)

    if focus_regions:
        plot_widget.plot(*[(region['data'] - np.array((t,0),),
                            dict(tuplesize = -2,
                                 _with     = f"lines lw 2 lc \"{region['color_string']}\"")) \
                           for region in focus_regions])


def handle_event_image_widget(widget, event):
    if event != FL_PUSH:
        return None

    if Fl.event_button() != FL_RIGHT_MOUSE:
        return None

    try:
        q = np.round(np.array(widget.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), ))).astype(int)
    except:
        return None

    if q is None:
        return None



    global focus_regions
    global imagersize

    q00 = q - args.radius
    q11 = q + args.radius

    if imagersize is None             or \
       np.any(q00 < 0)                or \
       np.any(q11 < 0)                or \
       np.any(imagersize-1 - q00 < 0) or \
       np.any(imagersize-1 - q11 < 0):
        return None


    # New region
    focus_regions.append(dict())
    region = focus_regions[-1]

    region['roi'] = ( slice(q00[1],q11[1]),
                      slice(q00[0],q11[0]) )
    q01 = np.array((q00[0], q11[1]))
    q10 = np.array((q11[0], q00[1]))

    color_string = color_sequence_rgb[ (len(focus_regions)-1) %
                                      len(color_sequence_rgb)]
    color_array = color_array_from_string(color_string)
    region_marking = dict(points = nps.cat(nps.cat(q00,q01),
                                           nps.cat(q01,q11),
                                           nps.cat(q11,q10),
                                           nps.cat(q10,q00)).astype(np.float32),
                          color_rgb = color_array )
    region['region_marking'] = region_marking
    region['color_string']   = color_string
    widget.set_lines( *[r['region_marking'] for r in focus_regions] )

    region['data'] = np.array(())

    return 1


kwargs = dict()
if args.dims is not None:
    kwargs['width' ] = args.dims[0]
    kwargs['height'] = args.dims[1]
if args.pixfmt is not None:
    kwargs['pixfmt'] = args.pixfmt
if args.trigger is not None:
    kwargs['trigger'] = args.trigger
if args.acquisition_mode is not None:
    kwargs['acquisition_mode'] = args.acquisition_mode
c = mrcam.camera(args.camera,
                 verbose = args.verbose,
                 recreate_stream_with_each_frame = args.recreate_stream_with_each_frame,
                 **kwargs)


W        = 1280
H        = 1024
window        = Fl_Window(W, H, "mrcam focus tool")
image_view_group = mrcam.Fl_Image_View_Group(0,0, W//2,H,
                                             camera          = c,
                                             single_buffered = args.single_buffered,
                                             features        = args.features,
                                             handle_extra    = handle_event_image_widget)
plot_widget   = Fl_Gnuplotlib_Window        (W//2, 0, W//2,H,
                                             ymin = 0,
                                             _xrange=(-args.plot_history,0)
                                             )
window.resizable(window)
window.end()
window.show()


image_view_group.set_up_image_capture(period         = args.period,
                                      image_callback = update_sharpness,
                                      flip_x         = args.flip_x,
                                      flip_y         = args.flip_y)

Fl.run()