#!/usr/bin/env python
"""
Library handling projection operations of a standard camera model.

Note: the image coordiante system has its origin at the center of the top left
pixel.

"""
from __future__ import division, print_function, absolute_import
import numpy as np
import cv2
import glob
import os
import copy
import shutil
from matplotlib import pyplot as plt
import time
from scipy.optimize import minimize
from scipy.interpolate import interp1d
import wx

# ROS imports
from sensor_models.nav_state import NavStateFixed, NavStateOdometry
from sensor_models.camera_models import load_from_file, AzelCamera, PTZCamera
from sensor_models.image_renderer import stitch_images, render_view
from wxpython_gui.manual_registration.gui import manual_registration
from tf.transformations import quaternion_multiply, quaternion_about_axis, \
    quaternion_inverse, quaternion_from_matrix


def homography_from_images(src_img, dst_img, initial_guess=None,
                           num_features=1000, num_levels=5, min_matches=10,
                           reproj_thresh=3, refine=True, multi_modal=False,
                           show_results=True):
    """
    Estimate the homography that warps image coordinates from the source image
    to the destination image.

    :param src_img: Source image.
    :type src_img: numpy.ndarray

    :param dst_img: Destination image.
    :type dst_img: numpy.ndarray

    :param initial_guess: Homography that warps the source image coordinates to
        destination image coordinates.
    :type initial_guess: 3x3 numpy.ndarray

    :param min_matches: Minimum number of matches required to accept the
        results.
    :type min_matches: int

    :param reproj_thresh:
    :type reproj_thresh: float

    :param refine: NOT IMPLEMENTED YET.
    :type refine: bool

    :param show_results: Show results.
    :type show_results: bool

    :return: List with the first element the homography that warps the source
        image to the destination image. The second element is the set of
        extracted feature coordinates in src_img, and the third element is the
        set of those matched-feature coordinates in dst_img.
    :rtype: list [Homography, 2xN array, 2xN array]

    """

    if src_img.ndim == 3:
        src_img = cv2.cvtColor(src_img, cv2.COLOR_RGB2GRAY)

    if dst_img.ndim == 3:
        dst_img = cv2.cvtColor(dst_img, cv2.COLOR_RGB2GRAY)

    # Find the keypoints and descriptors and match them.
    if cv2.__version__ == '2.4.8':
        orb = cv2.ORB(nfeatures=num_features, nlevels=num_levels,
                      scoreType=cv2.ORB_FAST_SCORE)
    else:
        orb = cv2.ORB_create(nfeatures=num_features,
                             scoreType=cv2.ORB_FAST_SCORE)

    if initial_guess is None:
        src_img2 = src_img
        dst_img2 = dst_img
    else:
        try:
            initial_guess_inv = np.linalg.inv(initial_guess)
        except:
            return None

        if np.prod(np.diag(initial_guess)) < 1:
            # src_image covers a small portion of dst_img, so we will take a
            # portion out of dst_img.
            dsize = tuple(src_img.shape[:2][::-1])
            dst_img2 = cv2.warpPerspective(dst_img, initial_guess_inv,
                                           dsize=dsize)
            src_img2 = src_img
        else:
            # dst_img covers a small portion of src_img, so we will take a
            # portion out of src_img.
            dsize = tuple(dst_img.shape[:2][::-1])
            src_img2 = cv2.warpPerspective(src_img, initial_guess,
                                           dsize=dsize)
            dst_img2 = dst_img

    #plt.figure(); imshow(src_img2); plt.figure(); imshow(dst_img2)
    kp1, des1 = orb.detectAndCompute(src_img2, None)
    kp2, des2 = orb.detectAndCompute(dst_img2, None)

    if multi_modal:
        # Take the negative of the image, detect/describe features, and add to
        # the previous features.
        src_img2 = 255-src_img2
        ret = orb.detectAndCompute(src_img2, None)
        kp1 = kp1 + ret[0]
        des1 = np.vstack([des1,ret[1]])

        dst_img2 = 255-dst_img2
        ret = orb.detectAndCompute(dst_img2, None)
        kp2 = kp2 + ret[0]
        des2 = np.vstack([des2,ret[1]])

    bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)

    # Match descriptors.
    matches = bf.match(des1,des2)

    if len(matches) < min_matches:
        print(min_matches)
        print("Only found %d matches, could not proceed." % (len(matches)))
        return None

    src_pts = np.float32([kp1[m.queryIdx].pt for m in matches])
    dst_pts = np.float32([kp2[m.trainIdx].pt for m in matches])


    mask = np.zeros(len(dst_pts))
    H, mask = cv2.findHomography(src_pts.reshape(-1,1,2),
                                 dst_pts.reshape(-1,1,2),
                                 method=cv2.RANSAC,
                                 ransacReprojThreshold=reproj_thresh,
                                 mask=mask)
    mask = mask.ravel().astype(np.bool)
    src_pts = src_pts[mask].T
    dst_pts = dst_pts[mask].T

    if src_pts.shape[1] < min_matches:
        n = src_pts.shape[1]
        print("Only found %d matches, could not proceed." % (n))
        return None

    if initial_guess is not None:
        # Undo the pre-warping
        if np.prod(np.diag(initial_guess)) < 1:
            H = np.dot(initial_guess, H)
            dst_pts = np.vstack([dst_pts,np.ones(dst_pts.shape[1])])
            dst_pts = np.dot(initial_guess, dst_pts)
            dst_pts = dst_pts[:2]/dst_pts[2]
        else:
            H = np.dot(H, initial_guess)
            src_pts = np.vstack([src_pts,np.ones(src_pts.shape[1])])
            src_pts = np.dot(initial_guess_inv, src_pts)
            src_pts = src_pts[:2]/src_pts[2]

    if show_results:
        plt.figure()
        plt.subplot('211')
        plt.imshow(src_img)
        plt.plot(src_pts[0], src_pts[1], 'ro')
        plt.title('Destination Image', fontsize=18)
        plt.subplot('212')
        plt.imshow(dst_img)
        plt.plot(dst_pts[0], dst_pts[1], 'bo')
        plt.title('Warped Version of Source Image', fontsize=18)

    return [H, src_pts, dst_pts]


def reproj_err(cm1, cm2, pt_corr, x=None, param_list=None, disp=1):
    """Reprojection error.

    :param cm1: Camera that may be optimized by x.
    :type cm1: instance of Camera

    :param cm2: Camera that may be a reference to cm1 or a camera that is not
        to be optimized.
    :type cm2: instance of Camera

    :param pt_corr: t1, t2, im_pts1, im_pts2

    """
    if x is not None:
        if cm1 == cm2:
            cm1 = copy.copy(cm1)
            cm1.set_param_array(param_list, x)
            cm2 = cm1
        else:
            cm1 = copy.copy(cm1)
            cm1.set_param_array(param_list, x)

    err = 0
    n = 0
    for i in range(len(pt_corr)):
        src_t,dst_t,src_pts,dst_pts = pt_corr[i]
        if src_pts.shape[1] > 0:
            ray_pos, ray_dir = cm1.unproject(src_pts, src_t)
            dst_pts2 = cm2.project(ray_dir*1000, dst_t)
            n += dst_pts2.shape[1]
            err += np.sum((dst_pts2-dst_pts).ravel()**2)

    err = np.sqrt(err/n)
    if disp == 1:
        print('Reprojection error:', err, 'pixels')

    return err


def reproj_err2(cm1, cm2, pt_corr):
    """Same as reproj_err but returns an array of errors.

    """
    err = np.zeros(0)
    for i in range(len(pt_corr)):
        src_t,dst_t,src_pts,dst_pts = pt_corr[i]
        if src_pts.shape[1] > 0:
            ray_pos, ray_dir = cm1.unproject(src_pts, src_t)
            dst_pts2 = cm2.project(ray_dir*1000, dst_t)
            erri = np.sqrt(np.sum((dst_pts2-dst_pts)**2,0))
        else:
            erri = np.zeros(0)

        err = np.hstack([err, erri])
    return err


def plot_reproj_err(cm1, cm2, pt_corr, num=10):
    for i in range(np.minimum(len(pt_corr), num)):
        plt.figure()
        src_t,dst_t,src_pts,dst_pts = pt_corr[i]
        if src_pts.shape[1] > 0:
            ray_pos, ray_dir = cm1.unproject(src_pts, src_t)
            dst_pts2 = cm2.project(ray_dir*1000, dst_t)
            try:
                img = cm2.get_image_from_list(dst_t)[0]
                plt.imshow(img, origin='upper', cmap='gray')
                plt.xlim([0,img.shape[1]])
                plt.ylim([img.shape[0],0])
            except:
                pass

            plt.plot(dst_pts[0], dst_pts[1], 'bo')
            plt.plot(dst_pts2[0], dst_pts2[1], 'ro')

            for j in range(dst_pts.shape[1]):
                plt.plot([dst_pts[0,j], dst_pts2[0,j]],
                         [dst_pts[1,j], dst_pts2[1,j]], 'g-')


def remove_outliers(cm1, cm2, pt_corr, err_thresh):
    """
    :param pt_corr: src_t, dst_t, src_pts, dst_pts

    """
    pt_corr_new = []
    for i in range(len(pt_corr)):
        src_t,dst_t,src_pts,dst_pts = pt_corr[i]
        ray_pos, ray_dir = cm1.unproject(src_pts, src_t)
        dst_pts2 = cm2.project(ray_dir*1000, dst_t)
        erri = np.sqrt(np.sum((dst_pts2-dst_pts)**2,0))
        ind = erri <= err_thresh
        if np.any(ind):
            src_pts = src_pts[:,ind]
            dst_pts = dst_pts[:,ind]
            pt_corr_new.append([src_t,dst_t,src_pts,dst_pts])

    return pt_corr_new


def k_mat_error(src_pts, dst_pts, K_ref, aspect_ratio, x=None,
                error_array=False):
    """Reprojection error for cameras with same pose but different K matrices.

    Both cameras are assumed to have the same pose but different K matrices,
    such is the case when matching between different zoom states of a zoom
    camera.

    :param cm1: Camera that may be optimized by x.
    :type cm1: instance of Camera

    :param cm2: Camera that may be a reference to cm1 or a camera that is not
        to be optimized.
    :type cm2: instance of Camera

    :param pt_corr: t1, t2, im_pts1, im_pts2

    """
    K = np.zeros((3,3))
    f = x[0]
    cx = x[1]
    cy = x[2]
    K = np.matrix([[f, 0, cx],
                   [0, f*aspect_ratio, cy],
                   [0, 0, 1]])

    src_pts = np.vstack([src_pts,np.ones(src_pts.shape[1])])
    dst_pts2 = np.asarray(np.dot(K, np.dot(np.linalg.inv(K_ref), src_pts)))
    dst_pts2 = dst_pts2[:2]/dst_pts2[2]
    err = np.sqrt(np.sum((dst_pts2 - dst_pts)**2, 0))
    if error_array:
        return err

    err = np.mean(err)
    #print('Reprojection error:', err, 'pixels')
    return err


def gradient_img(img, perc=0.1, blur_rad=1):
    grad_img = cv2.Laplacian(img, cv2.CV_64F)
    grad_img -= np.percentile(grad_img.ravel(), perc)
    grad_img /= np.percentile(grad_img.ravel(), 100-perc)/255
    grad_img = np.round(grad_img).astype(np.uint8)
    return cv2.GaussianBlur(grad_img, (blur_rad,blur_rad), 0)


def ptz_cal(ptz_cm_fname, frame_dir, num_features=3000, debug_images=True):
    """Calibrate the pan/tilt/zoom camera.

    This function expects a directory frame_dir that contains two
    subdirectories, 'pan_tilt_series' and 'zoom_series'.

    Note: the system base should have been stationary during the data collect.

    Pan/Tilt Calibration
    The first part of the calibration focuses on a fixed zoom equal to the
    camera's reference zoom. It will calibrate the intrinsic parameters (focal
    length, aspect ratio, principal point) at this zoom state. The subdirectory
    'pan_tilt_series' should contain a sequence of tif images acquired with a
    fixed zoom, equal to the reference zoom, and various pan and tilt values
    producing shifted frames with sufficient overlap. Points will be extracted
    from the images and matched from the overlap regions to build up a
    correspondence graph. The debug_images directory will be created showing
    all of the image point correspondences found. The intrinsic parameters of
    the camera will be optimized to minimize reprojection error. When this
    portion of the calibration completes, the image 'pt_stitched_mosaic.jpg'
    will be created, which shows a stitched version of the source frames using
    the optimized camera parameters. Sucess is indicated by very minimal seam
    artificat between the source frames.

    Note: the angular scale encoded within the intrinsic parameters derives
    from the pan/tilt angles of the system. Therefore, if these values are
    incorrectly scaled, the camera calibration will be likewise incorrect.

    Zoom Calibration
    An image acquired at the reference zoom along with the full set of
    calibrated intrinsic parameters for this zoom are subsequently used as a
    reference to calibrate camera parameters at other zoom states. Homographies
    are calculated between the reference zoom image and each other zoom image.
    These homographies are then used to extract the intrinsic camera matrix for
    the associated zom state. The intrinsic parameters are extracted from each
    camera matrix and a polynomial is fit against zoom state. When this stage
    of calibration completes, you should examine the newly created debug_images
    directory. Each image shows the reference zoom image adjacent to a version
    of an image from another zoom scaled using the calibrated parameters. The
    two images should have the same scale and registration.

    :param ptz_cm_fname: Filename for the PTZ camera yaml config file.
    :type ptz_cm_fname: str

    :param pt_frame_dir: Directory containing subdirectories 'pan_tilt_series'
        and 'zoom_series', which contain images acquired with different
        pan/tilt/zoom values. Each filename in the directory should be of the
        form '[pan],[tilt],[zoom].tif'.
    :type pt_frame_dir: str

    :param zoom_frame_dir: Directory containing images where the pan/tilt/zoom
        camera is set to a fixed pan and tilt and the zoom is varied over its
        range. Each filename in the directory should be of the form
        '[pan],[tilt],[zoom].jpg'.
    :type pt_frame_dir: str

    """
    if False:
        num_features = 3000
        debug_images = True
        frame_dir = '/home/mattb/threatx_ws/collected_data/calibration_data/support'
        ptz_cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/support_camera_flirptu_sony.yaml'

    cm = load_from_file(ptz_cm_fname)

    # In case ROS is running and publishing on the pan/tilt/zoom state topics.
    cm.ros_unregister_all()
    cm.clear_pt_time_series()
    cm.zoom_time_series()

    # ------------------------------------------------------------------------
    # Load in pan/tilt series of frames.
    pt_frame_dir = ''.join([frame_dir,'/pan_tilt_series'])
    img_fnames = glob.glob(''.join([pt_frame_dir,'/*.tif']))
    img_fnames.sort()
    t = 0   # Each image with its ptz state will be assigned a unique time.
    for i in range(len(img_fnames)):
        img = cv2.imread(img_fnames[i])
        if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        cm.add_image_to_list(img, t)
        fname = os.path.split(img_fnames[i])[1].split('.tif')[0]
        pan,tilt,zoom = fname.split(',')
        cm.add_pt_state(float(pan), float(tilt), t)
        cm.add_zoom_state(float(zoom), t)
        t += 1

    assert t > 0, ''.join([pt_frame_dir,' does not contain images.'])
    num_times = t

    # Set default values
    cm.width = img.shape[1]
    cm.height = img.shape[0]
    cm.pcx = cm.width/2
    cm.pcy = cm.height/2
    cm.aspect_ratio = 1
    cm.pf = 1000
    cm.pk1 = 0
    cm.pk2 = 0
    cm.pp1 = 0
    cm.pp2 = 0
    cm.pk3 = 0

    if debug_images:
        debug_dir = ''.join([pt_frame_dir,'/debug_images'])
        try:
            shutil.rmtree(debug_dir)
        except OSError:
            pass

        try:
            os.makedirs(debug_dir)
        except OSError:
            pass
    # ------------------------------------------------------------------------


    # ------------------------------------------------------------------------
    # Build up measurements from pan/tilt series
    print('Processing pan/tilt series')
    pt_corr = []
    for ti in range(num_times-1):
        for tj in np.arange(ti+1, num_times):
            print('Matching pan/tilt series frame {} on {}'.format(ti,tj))
            # Start by finding likely points in frame[0] that correspond to
            # frame[i]. We can use a homography fitting with RANSAC to identify
            # major outliers.
            tic = time.time()
            src_img = cm.get_image_from_list(ti)[0]
            dst_img = cm.get_image_from_list(tj)[0]

            initial_guess = None
            for i in range(2):
                ret = homography_from_images(src_img, dst_img,
                                             initial_guess=initial_guess,
                                             num_features=num_features,
                                             num_levels=10,
                                             min_matches=100, reproj_thresh=5,
                                             show_results=False)
                if ret is not None:
                    initial_guess = ret[0]

                break

            if ret is not None:
                H, src_pts, dst_pts  = ret

                if False:
                    # Prune down to the top N matches.
                    v = np.dot(H, np.vstack([src_pts,
                                             np.ones(src_pts.shape[1])]))
                    v = v[:2]/v[2]
                    err = np.sqrt(np.sum((v - dst_pts)**2, 0))
                    #ind = err < np.percentile(err, 50)
                    ind = np.argsort(err)[:np.minimum(300, len(err))]
                    src_pts = src_pts[:,ind]
                    dst_pts = dst_pts[:,ind]

                if False:
                    # Randomly pick N matches.
                    N = 100
                    l = dst_pts.shape[1]
                    ind = np.arange(l)[:np.minimum(N, l)]
                    np.random.shuffle(ind)
                    src_pts = src_pts[:,ind]
                    dst_pts = dst_pts[:,ind]

                if debug_images:
                    # Save images of the warped view to disk.
                    dsize=(src_img)
                    dsize = list(src_img.shape)
                    dsize[:2] = dsize[:2][::-1]
                    dsize = tuple(dsize)
                    #dst_img2 = cv2.warpPerspective(src_img, H, dsize=dsize)
                    h,w = src_img.shape[:2]
                    img = np.zeros((h,w*2, 3), dtype=np.uint8)
                    img[:,:w,0] = img[:,:w,1] = img[:,:w,2] = src_img
                    img[:,w:,0] = img[:,w:,1] = img[:,w:,2] = dst_img
                    for i in range(dst_pts.shape[1]):
                        cv2.circle(img, tuple(src_pts[:,i]), 5, (255,0,0), -1)
                        cv2.circle(img[:,w:,:], tuple(dst_pts[:,i]), 5,
                                   (255,0,0), -1)

                    fname = ''.join([debug_dir,'/',str(ti),'_',str(tj),'.jpg'])
                    cv2.imwrite(fname, img)

                pt_corr.append([ti,tj,ret[1],ret[2]])

            #print('Time elapsed', time.time() - tic)

    assert len(pt_corr) > 0, 'No matches found.'
    # ------------------------------------------------------------------------


    # ------------------------------------------------------------------------
    # Start optimizing camera parameters to minimize reprojection error.
    param_list = ['aspect_ratio','pf']
    err = lambda x: reproj_err(cm, cm, pt_corr, x, param_list)
    x0 = cm.get_param_array(param_list)
    x = fmin(err, x0, maxfun=200, ftol=0.1, disp=1)
    cm.set_param_array(param_list, x)

    param_list = ['aspect_ratio','pf', 'cam_quat']
    err = lambda x: reproj_err(cm, cm, pt_corr, x, param_list)
    x0 = cm.get_param_array(param_list)
    x = fmin(err, x0, maxfun=200, ftol=0.1, disp=1)
    cm.set_param_array(param_list, x)

    errs = reproj_err2(cm, cm, pt_corr)
    err_thresh = np.percentile(errs, 80)*1.5
    #plt.plot(np.sort(errs))
    pt_corr = remove_outliers(cm, cm, pt_corr, err_thresh)

    param_list = ['aspect_ratio','pf', 'cam_quat']
    err = lambda x: reproj_err(cm, cm, pt_corr, x, param_list)
    x0 = cm.get_param_array(param_list)
    x = fmin(err, x0, maxfun=200, ftol=0.01, disp=1)
    cm.set_param_array(param_list, x)

    #plot_reproj_err(cm, cm, pt_corr, num=10)

    if debug_images:
        print('Rendering pt_stitched_mosaic.jpg')
        camera_list = [[cm,ti] for ti in range(num_times)]
        mosaic_cm = AzelCamera.fit_to_cameras(camera_list,
                                              attached_to_world=True)

        src_list = []
        for ti in range(num_times):
            ret = cm.get_image_from_list(ti)
            src_list.append([cm,ret[0],ret[1]])

        img = stitch_images(src_list, mosaic_cm, 0, interpolation=3,
                            block_size=10, homog_approx=False)
        fname = ''.join([debug_dir,'/pt_stitched_mosaic.jpg'])
        cv2.imwrite(fname, img)


    # ------------------------------------------------------------------------
    # Load in zoom series of frames.
    zoom_frame_dir = ''.join([frame_dir,'/zoom_series'])
    img_fnames = glob.glob(''.join([zoom_frame_dir,'/*.tif']))
    img_fnames.sort()
    zoom_img = []
    zoom_state = []
    clahe = cv2.createCLAHE(clipLimit=0.5, tileGridSize=(8,8))
    for i in range(len(img_fnames)):
        img = cv2.imread(img_fnames[i])
        if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # Add to BA problem for final optimization.
        cm.add_image_to_list(img, t)
        fname = os.path.split(img_fnames[i])[1].split('.tif')[0]
        pan,tilt,zoom = fname.split(',')
        pan = float(pan)
        tilt = float(tilt)
        zoom = float(zoom)

        zoom_img.append(clahe.apply(img))
        zoom_state.append(np.round(zoom))

    assert cm.ref_zoom in zoom_state, ''.join(['Reference zoom ',
                                               str(cm.ref_zoom),
                                               ' not in zoom image series.'])

    num_times = t

    if debug_images:
        debug_dir = ''.join([zoom_frame_dir,'/debug_images'])
        try:
            shutil.rmtree(debug_dir)
        except OSError:
            pass

        try:
            os.makedirs(debug_dir)
        except OSError:
            pass

    zoom_state = np.array(zoom_state)
    ind = np.argsort(zoom_state)
    zoom_state = zoom_state[ind]
    zoom_img = [zoom_img[i] for i in ind]
    homographies = [[[] for i in range(len(zoom_state))] for i in range(len(zoom_state))]
    #homographies[0] = np.identity(3)
    for i in np.arange(0, len(zoom_state)-1):
        for j in np.arange(i+1, np.minimum(len(zoom_state), i+3)):
            print('Matching zoom images Index {} ({}) on {} ({})'.format(i,
                  zoom_state[i],j,zoom_state[j]))
            tic = time.time()
            #plt.figure(); imshow(zoom_img[j]); plt.figure(); imshow(zoom_img[i])
            ret = homography_from_images(zoom_img[i], zoom_img[j],
                                         num_features=5000,
                                         num_levels=10,
                                         min_matches=100, reproj_thresh=10,
                                         show_results=False)

            if ret is not None:
                H, src_pts, dst_pts  = ret
                # H maps from j to i, we then have to map from i to
                # ref_zoom_ind to get the homography that maps from j to
                # ref_zoom_ind
                homographies[i][j] = H
            else:
                break

    # Check that each frame found at least one match.
    for i in np.arange(1, len(zoom_state)):
        all_empty = True
        for j in np.arange(0, len(zoom_state)):
            if len(homographies[j][i]) > 0:
                all_empty = False

        if all_empty:
            raise Exception(''.join(['Zoom state ',str(zoom_state[i]),' (',
                                     str(i+1),'/',str(len(zoom_state)),')',
                                     ' could not be matched to any other ',
                                     'frames. You must retry calibration with',
                                     ' a different look direction.']))

    # Fill in the rest by chaining together sequences of frames.
    k = 1
    while k < len(zoom_state):
        for i in np.arange(0, len(zoom_state)-1):
            j = i + k
            if j == len(zoom_state):
                break

            if len(homographies[i][j]) == 0:
                homographies[i][j] = np.dot(homographies[j-1][j],
                                            homographies[i][j-1])

        k += 1

    if False:
        # Debug view of the current coverage of pairs.
        mask = np.zeros((len(zoom_state),len(zoom_state)))
        for i in np.arange(0, len(zoom_state)):
            for j in np.arange(0, len(zoom_state)):
                mask[i,j] = len(homographies[i][j]) > 0

    homogs_from_ref = []
    ref_zoom_ind = int(np.where(zoom_state == cm.ref_zoom)[0])
    for i in range(len(zoom_state)):
        if i == ref_zoom_ind:
            homogs_from_ref.append(np.identity(3))
            continue

        if len(homographies[ref_zoom_ind][i]) == 0:
            homogs_from_ref.append(np.linalg.inv(homographies[i][ref_zoom_ind]))
        else:
            homogs_from_ref.append(homographies[ref_zoom_ind][i])


    # ------------------------------------------------------------------------
    # Do a final zoom homography pass using the existing good initial guess
    # and fit to camera matrix.
    K_ref = cm.intrinsics_at_zoom(cm.ref_zoom)[0]
    f = []
    cx = []
    cy = []
    i = ref_zoom_ind
    for j in np.arange(0, len(zoom_state)):
        if i == j:
            f.append(K_ref[0,0])
            cx.append(K_ref[0,2])
            cy.append(K_ref[1,2])
            continue

        print('Final pass matching zoom index {} ({}) on {} ({})'.format(i,
              zoom_state[i],j,zoom_state[j]))
        tic = time.time()
        initial_guess = homogs_from_ref[j]
        #plt.figure(); imshow(zoom_img[i]); plt.figure(); imshow(zoom_img[j])
        src_img, dst_img = zoom_img[i], zoom_img[j]
        ret = homography_from_images(src_img, dst_img,
                                     initial_guess=initial_guess,
                                     num_features=10000,
                                     num_levels=10,
                                     min_matches=100, reproj_thresh=5,
                                     show_results=False)

        if ret is not None:
            H, src_pts, dst_pts  = ret

            # Take pairwise homographies between jth zoom and the reference
            # zoom and extract the camera matrix and constituent parameters.
            Ki = np.dot(H, K_ref)
            Ki /= Ki[2,2]
            x = [np.mean([Ki[0,0],Ki[1,1]]),Ki[0,2],Ki[1,2]]

            err = lambda x: k_mat_error(src_pts, dst_pts, K_ref, cm.aspect_ratio, x)
            x = fmin(err, x, maxfun=200, ftol=0.1, disp=0)

            err = k_mat_error(src_pts, dst_pts, K_ref, cm.aspect_ratio, x, error_array=True)
            ind = err < np.percentile(err, 80)*1.2
            src_pts = src_pts[:,ind]
            dst_pts = dst_pts[:,ind]

            err = lambda x: k_mat_error(src_pts, dst_pts, K_ref, cm.aspect_ratio, x)
            x = fmin(err, x, maxfun=200, ftol=0.1, disp=0)

            f.append(x[0])
            cx.append(x[1])
            cy.append(x[2])

            if debug_images:
                H = homogs_from_ref[j]
                if np.prod(np.diag(H)) < 1:
                    # src_image covers a small portion of dst_img
                    Hinv = np.linalg.inv(H)
                    dsize = tuple(src_img.shape[:2][::-1])
                    dst_img = cv2.warpPerspective(dst_img, Hinv, dsize=dsize)
                    dst_pts = np.vstack([dst_pts,np.ones(dst_pts.shape[1])])
                    dst_pts = np.dot(Hinv, dst_pts)
                    dst_pts = dst_pts[:2]/dst_pts[2]
                else:
                    # dst_img covers a small portion of src_img
                    dsize = tuple(dst_img.shape[:2][::-1])
                    src_img = cv2.warpPerspective(src_img, H,
                                                  dsize=dsize)
                    src_pts = np.vstack([src_pts,np.ones(src_pts.shape[1])])
                    src_pts = np.dot(H, src_pts)
                    src_pts = src_pts[:2]/src_pts[2]

                # Save images of the warped view to disk.
                dsize=(src_img)
                dsize = list(src_img.shape)
                dsize[:2] = dsize[:2][::-1]
                dsize = tuple(dsize)
                h,w = src_img.shape[:2]
                img = np.zeros((h,w*2, 3), dtype=np.uint8)
                img[:,:w,0] = img[:,:w,1] = img[:,:w,2] = src_img
                img[:,w:,0] = img[:,w:,1] = img[:,w:,2] = dst_img
                for k in range(dst_pts.shape[1]):
                    cv2.circle(img, tuple(np.round(src_pts[:,k]).astype(np.int)),
                               5, (255,0,0), -1)
                    cv2.circle(img[:,w:,:],
                               tuple(np.round(dst_pts[:,k]).astype(np.int)), 5,
                               (255,0,0), -1)

                fname = ''.join([debug_dir,'/',str(zoom_state[i]),'_on_',
                                 str(zoom_state[j]),'.jpg'])
                cv2.imwrite(fname, img)
        else:
            print("Using initial guess instead.")
            # Just use the initial guess
            Ki = np.dot(initial_guess, K_ref)
            Ki /= Ki[2,2]
            x = [np.mean([Ki[0,0],Ki[1,1]]),Ki[0,2],Ki[1,2]]
            f.append(x[0])
            cx.append(x[1])
            cy.append(x[2])
    # ------------------------------------------------------------------------

    f = np.array(f)
    cx = np.array(cx)
    cy = np.array(cy)

    # Upsample before fitting so that we can use a higher order fit.
    if True:
        n = 10000
        z = np.linspace(zoom_state[0], zoom_state[-1], n)
        interp = interp1d(zoom_state, f, kind='linear')
        fi = interp(z)
        interp = interp1d(zoom_state, cx, kind='linear')
        cxi = interp(z)
        interp = interp1d(zoom_state, cy, kind='linear')
        cyi = interp(z)
    else:
        z = zoom_state
        fi = f
        cxi = cx
        cyi =cy

    cm.pf = np.polyfit(z, fi,15)
    cm.pcx = np.polyfit(z, cxi, 12)
    cm.pcy = np.polyfit(z, cyi, 12)

    if True:
        plt.figure()
        plt.subplot(211)
        plt.plot(zoom_state, f, 'ro')
        plt.plot(z, np.polyval(cm.pf, z), 'b-')
        plt.xlabel('Zoom State', fontsize=18)
        plt.ylabel('Focal Length', fontsize=18)
        plt.subplot(212)
        err = (fi - np.polyval(cm.pf, z))/fi*100
        plt.plot(z, err, 'b-')
        plt.xlabel('Zoom State', fontsize=18)
        plt.ylabel('Focal Length Error (%)', fontsize=18)

        plt.figure()
        plt.plot(zoom_state, cx, 'ro')
        plt.plot(zoom_state, np.polyval(cm.pcx, zoom_state), 'b-')
        plt.xlabel('Zoom State', fontsize=18)
        plt.ylabel('Principal Point (x)', fontsize=18)
        plt.figure()
        plt.plot(zoom_state, cy, 'ro')
        plt.plot(zoom_state, np.polyval(cm.pcy, zoom_state), 'b-')
        plt.xlabel('Zoom State', fontsize=18)
        plt.ylabel('Principal Point (y)', fontsize=18)

    if True:
        # Final check
        K_ref = cm.intrinsics_at_zoom(cm.ref_zoom)[0]
        i = ref_zoom_ind
        for j in range(len(zoom_state)):
            src_img, dst_img = zoom_img[i], zoom_img[j]
            if i == j:
                continue

            Ki = cm.intrinsics_at_zoom(zoom_state[j])[0]
            H = np.dot(Ki, np.linalg.inv(K_ref))
            if np.prod(np.diag(H)) < 1:
                # src_image covers a small portion of dst_img
                Hinv = np.linalg.inv(H)
                dsize = tuple(src_img.shape[:2][::-1])
                dst_img = cv2.warpPerspective(dst_img, Hinv, dsize=dsize)
                dst_pts = np.vstack([dst_pts,np.ones(dst_pts.shape[1])])
                dst_pts = np.dot(Hinv, dst_pts)
                dst_pts = dst_pts[:2]/dst_pts[2]
                pts = cv2.goodFeaturesToTrack(src_img, 1000, 0.01, 10)
            else:
                # dst_img covers a small portion of src_img
                dsize = tuple(dst_img.shape[:2][::-1])
                src_img = cv2.warpPerspective(src_img, H,
                                              dsize=dsize)
                src_pts = np.vstack([src_pts,np.ones(src_pts.shape[1])])
                src_pts = np.dot(H, src_pts)
                src_pts = src_pts[:2]/src_pts[2]
                pts = cv2.goodFeaturesToTrack(dst_img, 1000, 0.01, 10)

            # Save images of the warped view to disk.
            pts = np.round(np.squeeze(pts, 1).T).astype(np.int)
            dsize=(src_img)
            dsize = list(src_img.shape)
            dsize[:2] = dsize[:2][::-1]
            dsize = tuple(dsize)
            h,w = src_img.shape[:2]
            img = np.zeros((h,w*2, 3), dtype=np.uint8)
            img[:,:w,0] = img[:,:w,1] = img[:,:w,2] = src_img
            img[:,w:,0] = img[:,w:,1] = img[:,w:,2] = dst_img
            for k in range(pts.shape[1]):
                cv2.circle(img, tuple(pts[:,k]), 5, (255,0,0), -1)
                cv2.circle(img[:,w:,:], tuple(pts[:,k]), 5, (255,0,0), -1)

            fname = ''.join([debug_dir,'/',str(zoom_state[i]),'_on_',
                             str(zoom_state[j]),'.jpg'])
            cv2.imwrite(fname, img)

    cm.save_to_file(ptz_cm_fname)
    print('Calibration finished successfully!!')


def ptz_to_ins_cal(ptz_cm_fname, data_fname, cueing_cm_fname_list=[],
                   base_quat_guess=[0,0.70710678,0.70710678,0]):
    """
    :param ptz_cm_fname: Filename for the PTZ camera yaml config file.
    :type ptz_cm_fname: str

    :data_fname: Calibration data .txt file with rows of the form
        [pan,tilt,nav_pos,nav_quat,target_enu] in degrees/meters.
    :data type: str

    :param cueing_cm_fname_list: Other camera models in the system, which were
        previously calibrated relative to the PTZ camera, that need to be
        rotated in unison with the PTZ camera.

    :type cueing_cm_fname_list: list of str

    :param base_quat_guess: Quaternion for the initial guess at base_quat. What
        is most important is that the z-axis of the coordinate system
        represented by this quaternion is aligned properly relative to the INS
        because this will not be updated. The optimization will rotate the
        camera around the INS z-axis only.
    :type base_quat_guess: 4-array

    """
    if False:
        ptz_cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/scout_camera_ptz_axis.yaml'
        data_fname = '/home/mattb/calibration_data/scout/ins_to_ptz_calibration_data.txt'
        base_quat_guess = [0,0.70710678,0.70710678,0]

    nav_state_provider = NavStateOdometry(None)
    cm = load_from_file(ptz_cm_fname, nav_state_provider)

    base_quat0 = copy.copy(cm.base_quat)

    # Set a reasonable level guess
    cm.base_quat = base_quat_guess


    data = np.loadtxt(data_fname)
    data = np.atleast_2d(data)

    # We assume the system was stationary during this collect.
    data[:,2:5] = np.mean(data[:,2:5], 0)

    # In case ROS is running and publishing on the pan/tilt/zoom state topics.
    cm.ros_unregister_all()
    cm.clear_pt_time_series()
    cm.zoom_time_series()

    for t in range(len(data)):
        pan = data[t][0]
        tilt = data[t][1]
        nav_pos = data[t][2:5]
        nav_quat = data[t][5:9]
        target_enu = data[t][9:]
        cm.add_pt_state(float(pan), float(tilt), t)
        cm.add_zoom_state(cm.max_zoom, t)
        nav_state_provider.add_to_pose_time_series(t, nav_pos, nav_quat)
        t += 1

    num_times = t

    target_ray = target_enu - nav_state_provider.average_pos()
    target_ray /= np.linalg.norm(target_ray)

    # Axis in the navigation coordinate system around which we allow the PTZ
    # camera coordinate system to rotate in order to optimize results.
    axis = [0,0,1]

    def cost_fun(x, cm):
        cm = copy.copy(cm)
        err = 0

        # Apply x, a rotation around axis of the PTZ base coordinate system.
        cm.base_quat = quaternion_multiply(quaternion_about_axis(x, axis),
                                           cm.base_quat)

        for t in range(num_times):
            ray_dir = cm.unproject([cm.width/2,cm.height/2], t)[1].ravel()
            dp = np.dot(ray_dir, target_ray)
            dp = np.maximum(np.minimum(dp, 1), -1)
            err += np.arccos(dp)*180/np.pi

        err /= num_times
        #print('Ray angle error:', err, 'degrees')
        return err

    err_min = np.inf
    for i in np.linspace(0, 2*np.pi, 1000):
        erri = cost_fun(i, cm)
        if erri < err_min:
            err_min = erri
            x = i

    x = fmin(cost_fun, x, args=(cm,), maxfun=200, ftol=0.01, disp=1)
    print('Ray angle error:', cost_fun(x, cm), 'degrees')
    base_quat1 = quaternion_multiply(quaternion_about_axis(x, axis),
                                       cm.base_quat)
    base_quat1 /= np.linalg.norm(base_quat1)

    # base_quat1 = dq*base_quat0
    dq = quaternion_multiply(base_quat1, quaternion_inverse(base_quat0))

    cm.base_quat = quaternion_multiply(dq, base_quat0)
    cm.save_to_file(ptz_cm_fname)

    # Rotate other cameras that were already calibrated to the PTZ camera.
    for fname in cueing_cm_fname_list:
        cc = load_from_file(fname)
        cc.cam_quat = quaternion_multiply(dq, cc.cam_quat)
        cc.save_to_file(fname)


def cueing_cal(ptz_cm_fname, cueing_cm_fname, ptz_frame_dir, cueing_img_fname,
               num_features=3000, ir=False, debug_images=True):
    """

    """
    if False:
        ptz_cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/support_camera_ptz_axis.yaml'
        ptz_frame_dir = '/home/mattb/calibration_data/support/calibration_data.20170621_1/pan_tilt_series'
        cueing_cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/support_camera_right.yaml'
        cueing_img_fname = '/home/mattb/calibration_data/support/calibration_data.20170621_1/support_camera_right.tif'
        num_features = 10000
        debug_images = True
        ir = False

    if ir:
        min_matches = 5
    else:
        min_matches = 100

    # Read in the cueing camera model and images.
    cueing_img = cv2.imread(cueing_img_fname)
    if cueing_img.ndim == 3:
        cueing_img = cv2.cvtColor(cueing_img, cv2.COLOR_BGR2GRAY)

    clahe = cv2.createCLAHE(clipLimit=0.5, tileGridSize=(8,8))
    cueing_img = clahe.apply(cueing_img)

    # Set a reasonable initial guess.
    cueing_cm = load_from_file(cueing_cm_fname)
    cueing_cm.width = cueing_img.shape[1]
    cueing_cm.height = cueing_img.shape[0]
    cueing_cm.K_no_skew = (1000,1000,cueing_cm.width/2,cueing_cm.height/2)
    cueing_cm.dist = 0

    # Read in the PTZ camera model and images.
    ptz_cm = load_from_file(ptz_cm_fname)

    # In case ROS is running and publishing on the pan/tilt/zoom state topics.
    ptz_cm.ros_unregister_all()
    ptz_cm.clear_pt_time_series()
    ptz_cm.zoom_time_series()

    img_fnames = glob.glob(''.join([ptz_frame_dir,'/*.tif']))
    img_fnames.sort()
    t = 0   # Each image with its ptz state will be assigned a unique time.
    for i in range(len(img_fnames)):
        img = cv2.imread(img_fnames[i])
        if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        img = clahe.apply(img)
        ptz_cm.add_image_to_list(img, t)
        fname = os.path.split(img_fnames[i])[1].split('.tif')[0]
        pan,tilt,zoom = fname.split(',')
        ptz_cm.add_pt_state(float(pan), float(tilt), t)
        ptz_cm.add_zoom_state(float(zoom), t)
        t += 1

    num_times = t

    if debug_images:
        debug_dir = ''.join([os.path.splitext(cueing_img_fname)[0],
                             '_debug_images'])
        try:
            shutil.rmtree(debug_dir)
        except OSError:
            pass

        try:
            os.makedirs(debug_dir)
        except OSError:
            pass

    # Build up measurements from pan/tilt series
    pt_corr = []
    for t in range(num_times):
        print('Measuring points against PTZ frame {}'.format(t+1))
        # Start by finding likely points in frame[0] that correspond to
        # frame[i]. We can use a homography fitting with RANSAC to identify
        # major outliers.
        tic = time.time()
        dst_img = ptz_cm.get_image_from_list(t)[0]

        initial_guess = None
        for i in range(2):
            ret = homography_from_images(cueing_img, dst_img,
                                         num_features=num_features,
                                         num_levels=20,
                                         min_matches=min_matches,
                                         reproj_thresh=10,
                                         initial_guess=initial_guess,
                                         multi_modal=ir,
                                         show_results=False)
            if ret is not None:
                if np.any(np.diag(ret[0] < 0)):
                    break

                initial_guess = ret[0]

            break

        if ret is not None:
            if np.any(np.diag(ret[0] < 0)):
                # Negatives on the diagonal are bad.
                continue

            H, src_pts, dst_pts  = ret
            if debug_images:
                # Save images of the warped view to disk.
                dsize = list(cueing_img.shape)
                dsize[:2] = dsize[:2][::-1]
                dsize = tuple(dsize)
                dst_img2 = cv2.warpPerspective(dst_img, np.linalg.inv(H),
                                               dsize=dsize)
                h,w = cueing_img.shape[:2]
                img = np.zeros((h,w*2, 3), dtype=np.uint8)
                img[:,:w,0] = img[:,:w,1] = img[:,:w,2] = cueing_img
                img[:,w:,0] = img[:,w:,1] = img[:,w:,2] = dst_img2
                for i in range(dst_pts.shape[1]):
                    cv2.circle(img, tuple(src_pts[:,i]), 5, (255,0,0), -1)
                    cv2.circle(img[:,w:,:], tuple(src_pts[:,i]), 5,
                               (255,0,0), -1)

                fname = ''.join([debug_dir,'/',str(t),'.jpg'])
                cv2.imwrite(fname, img)

            pt_corr.append([0,t,ret[1],ret[2]])

        #print('Time elapsed', time.time() - tic)


    param_list = ['K_no_skew']
    err = lambda x: reproj_err(cueing_cm, ptz_cm, pt_corr, x, param_list)
    x0 = cueing_cm.get_param_array(param_list)
    x = fmin(err, x0, maxfun=200, disp=1)
    cueing_cm.set_param_array(param_list, x)

    errs = reproj_err2(cueing_cm, ptz_cm, pt_corr)
    err_thresh = np.percentile(errs, 80)*1.2
    #plt.plot(np.sort(errs))
    pt_corr = remove_outliers(cueing_cm, ptz_cm, pt_corr, err_thresh)

    for i in range(2):
        param_list = ['K_no_skew','cam_quat','dist']
        err = lambda x: reproj_err(cueing_cm, ptz_cm, pt_corr, x, param_list)
        x0 = cueing_cm.get_param_array(param_list)
        x = fmin(err, x0, maxfun=200, disp=1)
        cueing_cm.set_param_array(param_list, x)

        errs = reproj_err2(cueing_cm, ptz_cm, pt_corr)
        err_thresh = np.percentile(errs, 80)*1.2
        #plt.plot(np.sort(errs))
        pt_corr = remove_outliers(cueing_cm, ptz_cm, pt_corr, err_thresh)

    cueing_cm.save_to_file(cueing_cm_fname)

    if True:
        # Show final results
        pts = cv2.goodFeaturesToTrack(cueing_img, 1000, 0.01, 10)
        pts = np.round(np.squeeze(pts, 1).T).astype(np.int)
        for t in range(num_times):
            ptz_img = ptz_cm.get_image_from_list(t)[0]
            ptz_img = render_view(ptz_cm, ptz_img, t, cueing_cm, 0, interpolation=1,
                                  block_size=10)[0]

            # Save images of the warped view to disk.
            h,w = cueing_img.shape[:2]
            img = np.zeros((h,w*2, 3), dtype=np.uint8)
            img[:,:w,0] = img[:,:w,1] = img[:,:w,2] = cueing_img
            img[:,w:,0] = img[:,w:,1] = img[:,w:,2] = ptz_img
            for i in range(pts.shape[1]):
                cv2.circle(img, tuple(pts[:,i]), 5, (255,0,0), -1)
                cv2.circle(img[:,w:,:], tuple(pts[:,i]), 5, (255,0,0), -1)

            fname = ''.join([debug_dir,'/',str(t),'.jpg'])
            cv2.imwrite(fname, img)


def manual_cueing_cal(ref_cm_fname, cm_fname, ref_img_fname,
                      img_fname, try_autocal=True, debug_images=True):
    """Manual registration between an uncalibrated and an calibrated camera.

    A camera is calibrated by comparing imagery to another previously
    calibrated camera.

    :param ref_cm_fname: Path to the reference camera's camera model.
    :type ref_cm_fname: str

    :param cm_fname: Path to the uncalibrated camera's camera model, which will
        be updated.
    :type cm_fname: str

    :param ref_img_fname: Path to image from referance camera aquired at the
        same time as the camera to be calibrated.
    :type ref_img_fname: str

    :param img_fname: Path to image from the camera to be calibration, which is
        aquired at the same time as the reference image.
    :type img_fname: str

    :param try_autocal: Try to automatically match image features between
        images.
    :type try_autocal: bool

    :param debug_images: Save debug images.
    :type debug_images: bool

    """
    if False:
        ref_cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/support_ee2/camera_flirptu_sony.yaml'
        ref_img_fname = '/home/mattb/threatx_ws/collected_data/calibration_data/support/camera_ir3/0.328520894051,-0.0190739501268,10152.0.tif'
        cm_fname = '/home/mattb/threatx_ws/src/kitware-ros-pkg/sensor_models/config/support_ee2/camera_ir3.yaml'
        img_fname = '/home/mattb/threatx_ws/collected_data/calibration_data/support/camera_ir3/camera_ir3.tif'
        debug_images = True

    nav_state_provider = NavStateFixed([0,0,0],[0,0,0,1])
    ref_cm = load_from_file(ref_cm_fname, nav_state_provider)

    if isinstance(ref_cm, PTZCamera):
        # In case ROS is running and publishing on the pan/tilt/zoom state
        # topics.
        ref_cm.ros_unregister_all()
        ref_cm.clear_pt_time_series()
        ref_cm.zoom_time_series()

        # If reference is a PTZ camera, extract pan/tilt/zoom from filename.
        fname = os.path.split(ref_img_fname)[1].split('.tif')[0]
        pan,tilt,zoom = fname.split(',')
        ref_cm.add_pt_state(float(pan), float(tilt), 0)
        ref_cm.add_zoom_state(float(zoom), 0)

    ref_img = cv2.imread(ref_img_fname)
    if ref_img.ndim == 3:
        ref_img = cv2.cvtColor(ref_img, cv2.COLOR_BGR2RGB)

    # Read in the cueing camera model and images.
    img = cv2.imread(img_fname)
    if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)

    if False:
        if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        clahe = cv2.createCLAHE(clipLimit=0.5, tileGridSize=(8,8))
        img = clahe.apply(img)

    points = None
    if try_autocal:
        print('Trying autocalibration')
        ret = homography_from_images(img, ref_img,
                                     num_features=10000,
                                     num_levels=5,
                                     min_matches=10,
                                     reproj_thresh=10,
                                     show_results=False)

        if ret is not None and np.all(np.diag(ret[0] > 0)):
            H, src_pts, dst_pts  = ret
            points = np.vstack([src_pts,dst_pts]).T
        else:
            print('Could not automatically find image point correspondences.')


    print('Starting manual calibration.')
    pts = manual_registration(img, ref_img, points=points,
                              title1='Cueing Camera', title2='PTZ Frame')

    if pts is not None:
        src_pts = pts[:,:2].T
        dst_pts = pts[:,2:].T
    else:
        return None

    # Point correspondences.
    pt_corr = [[0,0,src_pts,dst_pts]]

    # Set a reasonable initial guess.
    cueing_cm = load_from_file(cm_fname, nav_state_provider)
    cueing_cm.width = img.shape[1]
    cueing_cm.height = img.shape[0]

    # Warps from cueing image image to PTZ image.
    H = cv2.findHomography(src_pts.reshape(-1,1,2), dst_pts.reshape(-1,1,2),
                           method=cv2.RANSAC, ransacReprojThreshold=10)[0]
    if H is None:
        raise RuntimeError("Failed to estimate homography!")

    # ------------------------------------------------------------------------
    # Use PTZ camera model to determine the ifov and orientation of the cueing
    # camera.

    # Points on cueing camera image
    cx = cueing_cm.width/2
    cy = cueing_cm.height/2
    pt0 = [cx,cy,1]     # at center
    pt1 = [cx+1,cy,1]   # slightly right of center
    pt2 = [cx,cy+1,1]   # slightly above center

    # Same points mapped to equivalent on PTZ
    pt0 = np.dot(H, pt0)
    pt0 = (pt0/pt0[2])[:2]
    pt1 = np.dot(H, pt1)
    pt1 = (pt1/pt1[2])[:2]
    pt2 = np.dot(H, pt2)
    pt2 = (pt2/pt2[2])[:2]

    ray1 = ref_cm.unproject([cx,cy], 0)[1]
    ray1 /= np.sqrt(np.sum(ray1**2, 0))
    ray2 = ref_cm.unproject([cx,cy+1], 0)[1]
    ray2 /= np.sqrt(np.sum(ray2**2, 0))
    ray3 = ref_cm.unproject([cx+1,cy], 0)[1]
    ray3 /= np.sqrt(np.sum(ray3**2, 0))

    ifovx = np.arccos(np.dot(ray1.ravel(), ray3.ravel()))
    ifovy = np.arccos(np.dot(ray1.ravel(), ray2.ravel()))

    cueing_cm.K_no_skew = (1/ifovx,1/ifovy,cueing_cm.width/2,cueing_cm.height/2)
    cueing_cm.dist = 0

    # Camera coordinate system relative to INS (since the INS is at identity
    # orientation). Define camera axes using Gram-Schmidt normalization.
    zaxis = ray1
    yaxis = ray2 - np.dot(ray1.T, ray2)*ray1
    yaxis /= np.sqrt(np.sum(yaxis**2, 0))
    xaxis = ray3 - np.dot(zaxis.T, ray3)*zaxis
    xaxis /= np.sqrt(np.sum(xaxis**2, 0))
    xaxis = xaxis - np.dot(yaxis.T, xaxis)*yaxis
    xaxis /= np.sqrt(np.sum(xaxis**2, 0))

    R = np.identity(4)
    R[:3,:3] = np.hstack([xaxis,yaxis,zaxis])
    cueing_cm.cam_quat = quaternion_from_matrix(R)
    # ------------------------------------------------------------------------

    if debug_images:
        debug_dir = ''.join([os.path.splitext(img_fname)[0],
                             '_debug_images'])
        try:
            shutil.rmtree(debug_dir)
        except OSError:
            pass

        try:
            os.makedirs(debug_dir)
        except OSError:
            pass

    print('Optimizing camera parameters.')

    if False:
        # Initial random search to find a reasonable camera orientation.
        print('Getting a reasonable initial guess.')
        best_err = np.inf
        best_x = None
        for i in range(1000):
            x = np.random.rand(4)*2-1
            param_list = ['cam_quat']
            cueing_cm.set_param_array(param_list, x)
            #param_list = ['focal_length','aspect_ratio','cam_quat']
            err = lambda x: reproj_err(cueing_cm, ref_cm, pt_corr, x, param_list,
                                       disp=0)
            x = cueing_cm.get_param_array(param_list)
            x = fmin(err, x, maxfun=100, disp=0)
            if err(x) < best_err:
                best_err = err(x)
                best_x = x
                print('Current best error:', best_err, 'pixels')
                if best_err < 20:
                    break

        cueing_cm.set_param_array(param_list, best_x)
        print('Refining the optimization.')

    # Optimize camera matrix and camera orientation.
    param_list = ['focal_length','aspect_ratio','cam_quat']
    err = lambda x: reproj_err(cueing_cm, ref_cm, pt_corr, x, param_list,
                               disp=0)
    x = cueing_cm.get_param_array(param_list)
    err0 = 1e10
    k = 0
    tic = time.time()
    while (err0-err(x))/err0 > 1e-16:
        err0 = err(x)
        x = fmin(err, x, maxfun=1000, disp=0)
        print('Current best error:', err(x), 'pixels')
        cueing_cm.set_param_array(param_list, x)
        k += 1
        if time.time() - tic > 10:
            break

    print('Final error:', err(x), 'pixels')
    cueing_cm.save_to_file(cm_fname)

    if True:
        # Show final results
        if img.ndim == 3:
            img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        else:
            img = img
        pts = cv2.goodFeaturesToTrack(img, 1000, 0.01, 10)
        pts = np.round(np.squeeze(pts, 1).T).astype(np.int)

        ref_img = render_view(ref_cm, ref_img, 0, cueing_cm, 0,
                              interpolation=1,
                              block_size=10)[0]

        # Save images of the warped view to disk.
        h,w = img.shape[:2]
        debug_img = np.zeros((h,w*2, 3), dtype=np.uint8)

        if img.ndim == 3:
            debug_img[:,:w,:] = img
        else:
            debug_img[:,:w,0] = debug_img[:,:w,1] = debug_img[:,:w,2] = img

        if ref_img.ndim == 3:
            debug_img[:,w:,:] = ref_img
        else:
            debug_img[:,w:,0] = debug_img[:,w:,1] = debug_img[:,w:,2] = ref_img

        for i in range(pts.shape[1]):
            cv2.circle(debug_img, tuple(pts[:,i]), 5, (255,0,0), 1)
            cv2.circle(debug_img[:,w:,:], tuple(pts[:,i]), 5, (255,0,0), 1)

        fname = ''.join([debug_dir,'/registration.jpg'])

        if debug_img.ndim == 3:
            debug_img = cv2.cvtColor(debug_img, cv2.COLOR_RGB2BGR)

        cv2.imwrite(fname, debug_img)


def aligned_cropped_cueing_models(camera_fnames, mosaic_model_fname,
                                  cropped_camera_model_dir, pixels_high,
                                  show_plots=False):
    """Crop cueing cameras so that they are vertically aligned.

    :param camera_fnames: List of cueing camera filenames.
    :type camera_fnames: list of str

    :param mosaic_model_fname: Mosaic camera model filename.
    :type mosaic_model_fname: str

    :param cropped_camera_model_dir: Path to directory where the cropped camera
        models should be saved.
    :type cropped_camera_model_dir: str

    :param pixels_high: Number of rows in the cropped images.
    :type pixels_high: int

    """
    # Define a navigation provider that always returns a level vehicle with
    # zero heading. In other words, the vehicle x, y, and z axes align with the
    # world y, x, -z axes respectively.
    nav_state_provider = NavStateFixed(pos=np.array([0,0,0]),
                                       quat=np.array([np.sqrt(2)/2,
                                                      np.sqrt(2)/2,0,0]))

    # Add in each physical camera
    camera_list = []
    for cm_fname in camera_fnames:
        camera_list.append([load_from_file(cm_fname, nav_state_provider),0])

    # Fit mosaic to cameras
    print('Fitting mosaic to cameras.')
    mosaic = load_from_file(mosaic_model_fname)

    quaternion = [np.sqrt(2)/2,np.sqrt(2)/2,0,0]
    azel0 = AzelCamera(1, 1, -np.pi, np.pi, -np.pi/2, np.pi/2, quaternion,
                       None, None, nav_state_provider,
                       attached_to_world=False)

    min_az = np.inf
    max_az = -np.inf
    min_el = np.inf
    max_el = -np.inf
    ifov = []
    for i in range(len(camera_list)):
        assert isinstance(camera_list[i], list)
        cam = camera_list[i][0]
        t = camera_list[i][1]
        ifov.append(cam.ifov(t))
        x = range(cam.width)
        y = range(cam.height)
        xs = np.hstack([x,np.ones(len(y))*x[-1],x[::-1],np.ones(len(y))*x[0]])
        ys = np.hstack([np.ones(len(x))*y[0],y,np.ones(len(x))*y[-1],y[::-1]])
        im_pts = np.vstack([xs,ys])
        ray_pos, ray_dir = cam.unproject(im_pts, t)
        points = ray_pos + ray_dir*1e5
        azel_pts = azel0.img_to_azel(azel0.project(points, t))
        if show_plots:
            plt.plot(azel_pts[0]*180/np.pi, azel_pts[1]*180/np.pi)
        min_az = np.minimum(np.min(azel_pts[0]), min_az)
        max_az = np.maximum(np.max(azel_pts[0]), max_az)
        min_el = np.minimum(np.min(azel_pts[1]), min_el)
        max_el = np.maximum(np.max(azel_pts[1]), max_el)

    el = (max_el + min_el)/2
    N = 5000
    azel = np.vstack([np.linspace(min_az, max_az, N),
                      np.ones(N)*el])

    y_centers = []
    for i in range(len(camera_list)):
        assert isinstance(camera_list[i], list)
        cam = camera_list[i][0]
        t = camera_list[i][1]
        im_pts = azel0.azel_to_img(azel)
        ray_pos, ray_dir = azel0.unproject(im_pts, t)
        im_pts = cam.project(ray_pos + ray_dir*1e5, t)
        ind = np.logical_and(im_pts[0] > 0, im_pts[1] > 0)
        ind = np.logical_and(ind, im_pts[0] < cam.width)
        ind = np.logical_and(ind, im_pts[1] < cam.height)
        im_pts = im_pts[:,ind]
        #plt.figure()
        #plt.plot(im_pts[0], im_pts[1])
        y_centers.append(np.median(im_pts[1]))

    y_centers = np.round(y_centers)

    tpixels = pixels_high//2
    bpixels = pixels_high - tpixels
    y_tops = y_centers - tpixels
    y_bots = y_centers + bpixels

    with open('%s/camera_image_crops.txt' % cropped_camera_model_dir, 'w') as f:
        for i in range(len(y_tops)):
            line = 'Camera %i: %i:%i' % (i,y_tops[i],y_bots[i])
            print(line)
            f.write('%s\n' % line)

    # Save cueing camera models.
    for i in range(len(camera_fnames)):
        fname = camera_fnames[i]
        head,tail = os.path.split(fname)
        fname = '%s/%s' % (cropped_camera_model_dir,tail)
        cam = camera_list[i][0]
        cam.height = pixels_high
        K = cam.K_no_skew
        K[-1] -= y_tops[i]
        cam.K_no_skew = K
        cam.save_to_file(fname)

    # ------------------------------------------------------------------------
    # Find top of mosaic.
    max_el = np.inf
    for i in range(len(camera_list)):
        assert isinstance(camera_list[i], list)
        cam = camera_list[i][0]
        t = camera_list[i][1]
        x = range(cam.width)[5:-5]
        y = range(cam.height)[5:-5]
        xs = x
        ys = np.ones(len(x))*y[0]
        im_pts = np.vstack([xs,ys])
        ray_pos, ray_dir = cam.unproject(im_pts, t)
        points = ray_pos + ray_dir*1e5
        azel_pts = azel0.img_to_azel(azel0.project(points, t))
        #plt.plot(azel_pts[0]*180/np.pi, azel_pts[1]*180/np.pi)
        max_el = np.minimum(np.max(azel_pts[1]), max_el)

    # Find bottom of mosaic.
    min_el = -np.inf
    for i in range(len(camera_list)):
        assert isinstance(camera_list[i], list)
        cam = camera_list[i][0]
        t = camera_list[i][1]
        x = range(cam.width)
        y = range(cam.height)
        xs = x
        ys = np.ones(len(x))*y[-1]
        im_pts = np.vstack([xs,ys])
        ray_pos, ray_dir = cam.unproject(im_pts, t)
        points = ray_pos + ray_dir*1e5
        azel_pts = azel0.img_to_azel(azel0.project(points, t))
        #plt.plot(azel_pts[0]*180/np.pi, azel_pts[1]*180/np.pi)
        min_el = np.maximum(np.min(azel_pts[1]), min_el)

    # Find left and right edges of mosaic.
    min_az = -np.inf
    cam = camera_list[0][0]
    t = camera_list[0][1]
    x = range(cam.width)
    y = range(cam.height)
    xs = np.ones(len(y))*x[0]
    ys = y
    im_pts = np.vstack([xs,ys])
    ray_pos, ray_dir = cam.unproject(im_pts, t)
    points = ray_pos + ray_dir*1e5
    azel_pts = azel0.img_to_azel(azel0.project(points, t))
    #plt.plot(azel_pts[0]*180/np.pi, azel_pts[1]*180/np.pi)
    min_az = np.maximum(np.min(azel_pts[0]), min_az)

    # Find mosaic right edge.
    max_az = np.inf
    cam = camera_list[-1][0]
    t = camera_list[-1][1]
    x = range(cam.width)
    y = range(cam.height)
    xs = np.ones(len(y))*x[-1]
    ys = y
    im_pts = np.vstack([xs,ys])
    ray_pos, ray_dir = cam.unproject(im_pts, t)
    points = ray_pos + ray_dir*1e5
    azel_pts = azel0.img_to_azel(azel0.project(points, t))
    #plt.plot(azel_pts[0]*180/np.pi, azel_pts[1]*180/np.pi)
    max_az = np.minimum(np.max(azel_pts[0]), max_az)

    downsample = 2
    ifov = np.median(np.array(ifov))*downsample
    mosaic.width = int((max_az-min_az)//ifov)
    mosaic.height = int((max_el-min_el)//ifov)
    mosaic._min_el = min_el
    mosaic._max_el = max_el
    mosaic._min_az = min_az
    mosaic._max_az = max_az
    mosaic._quaternion = quaternion

    head,tail = os.path.split(mosaic_model_fname)
    fname = '%s/%s' % (cropped_camera_model_dir,tail)
    mosaic.save_to_file(fname)
