#!/usr/bin/env python
"""
Library handling projection operations of a standard camera model.
"""
from __future__ import division, print_function
import os
import time
import glob
from PIL import Image
import numpy as np
import cv2

import pygeodesy
from osgeo import osr, gdal
import simplekml

# ROS imports
from tf.transformations import quaternion_multiply, quaternion_matrix, \
    quaternion_from_euler, euler_from_quaternion, quaternion_from_matrix, \
    quaternion_inverse, euler_matrix, quaternion_slerp, random_quaternion

# KAMERA imports.
from sensor_models.nav_conversions import enu_to_llh, llh_to_enu


# Get noaa_kamera path.
try:
    try:
        path = os.path.realpath(__file__)
        ws_path = '/'.join(os.path.split(path)[0].split('/')[:-5])
    except NameError:
        if os.path.isdir(os.path.expanduser('~/kamera_ws')):
            ws_path = os.path.expanduser('~/kamera_ws')
        elif os.path.isdir('/root/kamera_ws'):
            ws_path = '/root/kamera_ws'
        elif os.path.isdir('/home/kamera/noaa_kamera'):
            ws_path = '/home/kamera/noaa_kamera'


    geod_filename = '%s/src/kitware-ros-pkg/wxpython_gui/geods/egm84-15.pgm' % ws_path
    geod = pygeodesy.geoids.GeoidPGM(geod_filename)
except:
    pass

wgs84_cs = osr.SpatialReference()
wgs84_cs.SetWellKnownGeogCS("WGS84")
wgs84_wkt = wgs84_cs.ExportToPrettyWkt()


# Bayer pattern dictionary.
bayer_patterns = {}
bayer_patterns['bayer_rggb8'] = cv2.COLOR_BayerBG2RGB
bayer_patterns['bayer_grbg8'] = cv2.COLOR_BayerGB2RGB
bayer_patterns['bayer_bggr8'] = cv2.COLOR_BayerRG2RGB
bayer_patterns['bayer_gbrg8'] = cv2.COLOR_BayerGR2RGB
bayer_patterns['bayer_rggb16'] = cv2.COLOR_BayerBG2RGB
bayer_patterns['bayer_grbg16'] = cv2.COLOR_BayerGB2RGB
bayer_patterns['bayer_bggr16'] = cv2.COLOR_BayerRG2RGB
bayer_patterns['bayer_gbrg16'] = cv2.COLOR_BayerGR2RGB


kml_color_map = {'left_rgb': simplekml.Color.green,
                 'center_rgb': simplekml.Color.green,
                 'right_rgb': simplekml.Color.green,
                 'left_ir': simplekml.Color.red,
                 'center_ir': simplekml.Color.red,
                 'right_ir': simplekml.Color.red,
                 'left_uv': simplekml.Color.yellow,
                 'center_uv': simplekml.Color.yellow,
                 'right_uv': simplekml.Color.yellow}


class Detection(object):
    __slots__ = ['uid', 'image_fname', 'frame_id', 'image_bbox', 'lonlat_bbox',
                 'confidence', 'length', 'confidence_pairs', 'gsd',
                 'height_meters', 'width_meters', 'suppressed']

    def __init__(self, uid, image_fname, frame_id, image_bbox, lonlat_bbox,
                 confidence, length, confidence_pairs, gsd, height_meters,
                 width_meters, suppressed):
        self.uid = uid
        self.image_fname = image_fname
        self.frame_id = frame_id
        self.image_bbox = image_bbox
        self.lonlat_bbox = lonlat_bbox
        self.confidence = confidence
        self.length = length
        self.confidence_pairs = confidence_pairs
        self.gsd = gsd
        self.height_meters = height_meters
        self.width_meters = width_meters
        self.suppressed = suppressed


def decompose_affine(A):
    '''Decompose homogenous affine transformation matrix `A` into parts.

    The transforms3d package, including all examples, code snippets and
    attached documentation is covered by the 2-clause BSD license.

    Copyright (c) 2009-2017, Matthew Brett and Christoph Gohlke
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are
    met:

    1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
    IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
    THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
    PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

    The parts are translations, rotations, zooms, shears.
    `A` can be any square matrix, but is typically shape (4,4).
    Decomposes A into ``T, R, Z, S``, such that, if A is shape (4,4)::
       Smat = np.array([[1, S[0], S[1]],
                        [0,    1, S[2]],
                        [0,    0,    1]])
       RZS = np.dot(R, np.dot(np.diag(Z), Smat))
       A = np.eye(4)
       A[:3,:3] = RZS
       A[:-1,-1] = T
    The order of transformations is therefore shears, followed by
    zooms, followed by rotations, followed by translations.
    The case above (A.shape == (4,4)) is the most common, and
    corresponds to a 3D affine, but in fact A need only be square.
    Parameters
    ----------
    A : array shape (N,N)
    Returns
    -------
    T : array, shape (N-1,)
       Translation vector
    R : array shape (N-1, N-1)
        rotation matrix
    Z : array, shape (N-1,)
       Zoom vector.  May have one negative zoom to prevent need for negative
       determinant R matrix above
    S : array, shape (P,)
       Shear vector, such that shears fill upper triangle above
       diagonal to form shear matrix.  P is the (N-2)th Triangular
       number, which happens to be 3 for a 4x4 affine.
    Examples
    --------
    >>> T = [20, 30, 40] # translations
    >>> R = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] # rotation matrix
    >>> Z = [2.0, 3.0, 4.0] # zooms
    >>> S = [0.2, 0.1, 0.3] # shears
    >>> # Now we make an affine matrix
    >>> A = np.eye(4)
    >>> Smat = np.array([[1, S[0], S[1]],
    ...                  [0,    1, S[2]],
    ...                  [0,    0,    1]])
    >>> RZS = np.dot(R, np.dot(np.diag(Z), Smat))
    >>> A[:3,:3] = RZS
    >>> A[:-1,-1] = T # set translations
    >>> Tdash, Rdash, Zdash, Sdash = decompose(A)
    >>> np.allclose(T, Tdash)
    True
    >>> np.allclose(R, Rdash)
    True
    >>> np.allclose(Z, Zdash)
    True
    >>> np.allclose(S, Sdash)
    True
    Notes
    -----
    We have used a nice trick from SPM to get the shears.  Let us call the
    starting N-1 by N-1 matrix ``RZS``, because it is the composition of the
    rotations on the zooms on the shears.  The rotation matrix ``R`` must have
    the property ``np.dot(R.T, R) == np.eye(N-1)``.  Thus ``np.dot(RZS.T,
    RZS)`` will, by the transpose rules, be equal to ``np.dot((ZS).T, (ZS))``.
    Because we are doing shears with the upper right part of the matrix, that
    means that the Cholesky decomposition of ``np.dot(RZS.T, RZS)`` will give
    us our ``ZS`` matrix, from which we take the zooms from the diagonal, and
    the shear values from the off-diagonal elements.
    '''
    A = np.asarray(A)
    T = A[:-1,-1]
    RZS = A[:-1,:-1]
    ZS = np.linalg.cholesky(np.dot(RZS.T,RZS)).T
    Z = np.diag(ZS).copy()
    shears = ZS / Z[:,np.newaxis]
    n = len(Z)
    S = shears[np.triu(np.ones((n,n)), 1).astype(bool)]
    R = np.dot(RZS, np.linalg.inv(ZS))
    if np.linalg.det(R) < 0:
        Z[0] *= -1
        ZS[0] *= -1
        R = np.dot(RZS, np.linalg.inv(ZS))
    return T, R, Z, S


def get_image_chip(image, left, right, top, bottom):
    l = np.maximum(left, 0)
    r = np.maximum(l, right)
    r = np.minimum(r, image.shape[1])
    t = np.maximum(top, 0)
    b = np.maximum(t, bottom)
    b = np.minimum(b, image.shape[0])

    if image.ndim == 3:
        return image[t:b, l:r, :]
    else:
        return image[t:b, l:r]


def points_along_image_border(width, height, num_points=4):
    """Return uniform array of points along the perimeter of the image.

    :param num_points: Number of points (approximately) to distribute
        around the perimeter of the image perimeter.
    :type num_points: int

    :return:
    :rtype: numpy.ndarry with shape (3,n)

    """
    perimeter = 2*(height + width)
    ds = num_points/float(perimeter)
    xn = np.max([2, int(ds*width)])
    yn = np.max([2, int(ds*height)])
    x = np.linspace(0, width, xn)
    y = np.linspace(0, height, yn)[1:-1]
    pts = np.vstack([np.hstack([x,
                                np.full(len(y), width,
                                        dtype=np.float64),
                                x[::-1],
                                np.zeros(len(y))]),
                     np.hstack([np.zeros(xn),
                                y,
                                np.full(xn, height,
                                        dtype=np.float64),
                                y[::-1]])])
    return pts


def meters_per_lon_lat(lon, lat):
    dtheta = 1e-5   # degrees
    meters_per_lat_deg = llh_to_enu(lat + dtheta, lon, 0, lat, lon, 0)[1]
    meters_per_lat_deg /= dtheta

    meters_per_lon_deg = llh_to_enu(lat, lon + dtheta, 0, lat, lon, 0)[0]
    meters_per_lon_deg /= dtheta

    return meters_per_lon_deg, meters_per_lat_deg


def get_image_boundary(camera_model, frame_time):
    """Return image coordinates and (latitude, longitude) for image border.

    This assumes the ground is located at mean sea level.

    """
    im_pts = np.array([[0, 0], [camera_model.width, 0],
                       [camera_model.width, camera_model.height],
                       [0, camera_model.height]])

    # nav_state_provider currently uses an assumption that we don't move too
    # far away from the origin, which is obviously violated here. Currently,
    # ypr is relative to the aircraft at its current state, which is fine in
    # this case.
    nav_state_provider = camera_model.nav_state_provider
    lat, lon, h = nav_state_provider.ins_llh(frame_time)

    # Location of geod (i.e., mean sea level, which is generally the ground for
    # us) relative to theellipsoid. Positive value means that mean sea level is
    # above the WGS84 ellipsoid.
    offset = geod.height(lat, lon)
    alt_msl = h - offset

    ray_pos, ray_dir = camera_model.unproject(im_pts.T, frame_time)
    ray_pos[:2] = 0
    ray_pos[2] = h
    corner_enu = (ray_pos + ray_dir*(-alt_msl/ray_dir[2])).T

    im_pts = im_pts.astype(np.float)

    corner_ll = []
    for i in range(len(corner_enu)):
        corner_ll.append(enu_to_llh(*corner_enu[i], lat0=lat, lon0=lon,
                                    h0=0)[:2])

    corner_ll = np.array(corner_ll, dtype=np.float)

    return im_pts, corner_ll


def measure_image_to_image_homographies(img_fnames, homog_out_dir,
                                        ins_homog_dir=None, num_features=10000,
                                        min_matches=40, reproj_thresh=5,
                                        save_viz_gif=False):
    if len(img_fnames) == 0:
        return

    # Try to read homographies in parallel dir.
    lon_lat_homog = {}

    if ins_homog_dir is not None:
        for fname in glob.glob('%s/*.txt' % ins_homog_dir):
            try:
                h = np.loadtxt(fname)
                img_fname = os.path.splitext(os.path.split(fname)[1])[0]
                lon_lat_homog[img_fname] = np.reshape(h, (3, 3), order='C')
            except:
                pass

    image_height, image_width = cv2.imread(img_fnames[0]).shape[:2]

    # Find the keypoints and descriptors and match them.
    orb = cv2.ORB_create(nfeatures=num_features, edgeThreshold=21,
                         patchSize=31, nlevels=16,
                         scoreType=cv2.ORB_FAST_SCORE, fastThreshold=10)

    fname0 = img_fnames[0]
    homog_ij = {}
    for i in range(len(img_fnames)):
        fname1 = os.path.splitext(os.path.split(img_fnames[i])[1])[0]

        fname_base = '%s_to_%s' % (fname0, fname1)
        fname_out = '%s/%s.txt' % (homog_out_dir, fname_base)

        print('Image %i/%i: extracting features from image \'%s\'' %
               (i + 1, len(img_fnames), fname1))

        h1 = lon_lat_homog.get(fname1, None)
        img1 = cv2.imread(img_fnames[i], -1)

        if img1.dtype == np.uint16:
            img1 = img1.astype(np.float)
            img1 -= np.percentile(img1.ravel(), 1)
            img1[img1 < 0] = 0
            img1 /= np.percentile(img1.ravel(), 99)/255
            img1[img1 > 225] = 255
            img1 = np.round(img1).astype(np.uint8)

        if img1.ndim == 3:
            img1_gray = cv2.cvtColor(img1, cv2.COLOR_RGB2GRAY)
        else:
            img1_gray = img1

        kp1, des1 = orb.detectAndCompute(img1_gray, None)

        if i == 0:
            fname0, h0, img0, kp0, des0 = fname1, h1, img1, kp1, des1
            continue

        bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        matches = bf.match(des0, des1)

        if len(matches) < min_matches:
            print('Only found %d feature matches, could not match images %i '
                   'to %i.' % (len(matches), i, i + 1))
            fname0, img0, kp0, des0 = fname1, img1, kp1, des1
            continue

        pts0 = np.float32([kp0[m.queryIdx].pt for m in matches])
        pts1 = np.float32([kp1[m.trainIdx].pt for m in matches])

        # Because this is an aircraft in forward flight, the optical flow
        # should predominantly be a translation. Let's say, conservatively,
        # that the displacement fits a translation within 1/20 the image's
        # larger dimension.
        if False:
            thresh = max([image_height, image_width])/20
            dxy = pts0 - pts1

            num_inliers = np.zeros(len(dxy))
            for k in range(len(dxy)):
                num_inliers[k] = sum(np.all(np.abs(dxy[i] - dxy) < thresh,
                                            axis=1))

            ind = np.all(dxy[np.argmax(num_inliers)] - dxy < thresh, axis=1)

            pts0 = pts0[ind]
            pts1 = pts1[ind]

            if len(pts0) < min_matches:
                print('Only found %d matches, could not match images %i to '
                       '%i.' % (len(pts0), i, i + 1))
                fname0, img0, kp0, des0 = fname1, img1, kp1, des1
                continue

        # ----------------------- Consider INS estimate ----------------------
        if h0 is not None and h1 is not None:
            # Homography warping from image i - 1 to image i coordinates.
            h = np.dot(np.linalg.inv(h1), h0)

            pts0_to_1 = np.dot(h, np.hstack([pts0, np.ones((len(pts0), 1))]).T)
            pts0_to_1 = (pts0_to_1[:2]/pts0_to_1[2]).T

            err = np.sqrt(np.sum((pts0_to_1 - pts1)**2, axis=1))

            # Calculate GSD
            xc = image_width/2
            yc = image_height/2
            lon_lats = np.dot(h0, np.array([[xc, xc + 1, xc],
                                            [yc, yc, yc + 1],
                                            [1, 1, 1]]))
            lon_lats = lon_lats[:2]/lon_lats[2]
            dx = llh_to_enu(lon_lats[1, 1], lon_lats[0, 1], 0,
                            lon_lats[1, 0], lon_lats[0, 0], 0)
            dx = np.linalg.norm(dx)
            dy = llh_to_enu(lon_lats[1, 2], lon_lats[0, 2], 0,
                            lon_lats[1, 0], lon_lats[0, 0], 0)
            dy = np.linalg.norm(dy)
            gsd = np.mean([dx, dy])

            # Assume 100 meters of geo-accuracy.
            thresh = 100/gsd

            ind = err < thresh
            pts0 = pts0[ind]
            pts1 = pts1[ind]

        def affine_not_valid(h):
            """Is this affine matrix valid for our flight motion.

            """
            if h is None:
                return True

            if len(h) == 2:
                h = np.vstack([h, [0, 0, 1]])

            try:
                translation, R, scale, S = decompose_affine(h)
            except:
                return True

            #translation = h[:2, 2]
            #scale = np.sqrt(np.sum(h[:, :2]**2, axis=0))
            #angle = (np.arccos(h[0, 0] / scale[0]))*180/np.pi
            angle = np.arctan2(R[1, 0], R[0, 0])*180/np.pi

            shear_angle = float(np.arctan(S)*180/np.pi)

            if min(scale) < 0.8 or max(scale) > 1.3:
                return True

            if abs(angle) > 30:
                return True

            if np.linalg.norm(translation) < 50:
                return False

            if abs(shear_angle) > 10:
                return True

            return False

        # Use OpenCV homogrpahy fitting because it is fast, but it implicitly
        # uses a reprojection threshold of 2, which may be too strict.
        h = cv2.estimateRigidTransform(pts0, pts1, False)
        pts0h = np.vstack([pts0.T, np.ones(len(pts0))])

        h_valid = not affine_not_valid(h)
        if h_valid:
            pts = np.dot(h, pts0h)
            pts = pts[:2]
            mask = np.sum((pts - pts1.T)**2, 0) < (2*reproj_thresh)**2
            h_valid = sum(mask) >= min_matches

        if not h_valid:
            # RANSAC loops
            L = len(pts0)
            h = None
            num_inliers = 0
            mask = np.zeros(len(pts0), np.bool)
            k = 0
            tic = time.time()
            k2 = 0
            while k < 20000:
                k += 1
                k2 += 1

                if k2 == 100:
                    k2 = 0
                    if time.time() > tic + 5:
                        break

                inds = np.random.randint(L, size=3)
                if set(inds) < 3:
                    continue

                h_ = cv2.getAffineTransform(pts0[inds], pts1[inds])
                #h_ = cv2.getPerspectiveTransform(pts0[inds], pts1[inds])

                # Determine if this is an acceptable homography.
                if affine_not_valid(h_):
                    continue

                pts = np.dot(h_, pts0h)
                pts = pts[:2]
                ind_ = np.sum((pts - pts1.T)**2, 0) < (2*reproj_thresh)**2
                num_inliers_ = sum(ind_)
                if num_inliers_ > num_inliers:
                    num_inliers = num_inliers_
                    h = h_
                    mask = ind_

        pts0 = pts0[mask]
        pts1 = pts1[mask]

        if len(pts0) < min_matches:
            print("Only found %d matches, could not match images %i to %i." %
                   (len(pts0), i, i + 1))
            fname0, img0, kp0, des0 = fname1, img1, kp1, des1
            continue

        if True:
            # Refine with full homography.
            mask = np.zeros(len(pts0))
            h, mask = cv2.findHomography(pts0.reshape(-1, 1, 2),
                                         pts1.reshape(-1, 1, 2),
                                         method=cv2.RANSAC,
                                         ransacReprojThreshold=reproj_thresh,
                                         mask=mask)

            if h is None:
                print("Only found %d matches, could not match images %i to %i." %
                       (len(pts0), i, i + 1))
                fname0, img0, kp0, des0 = fname1, img1, kp1, des1
                continue

            mask = mask.ravel().astype(np.bool)

            # Verify whether homography is acceptable. If not, do RANSAC with
            # only acceptable test cases.
            det = np.linalg.det(h)

            pts0 = pts0[mask]
            pts1 = pts1[mask]

        if len(h) == 2:
            # Make a 3x3 affine homography.
            h = np.vstack([h, [0, 0, 1]])

        if False:
            plt.figure()
            plt.subplot('211')
            plt.imshow(img0, cmap='gray', interpolation='none')
            plt.plot(pts0.T[0], pts0.T[1], 'ro')
            plt.title(str(i - 1), fontsize=18)
            plt.subplot('212')
            plt.imshow(img1, cmap='gray', interpolation='none')
            plt.plot(pts1.T[0], pts1.T[1], 'bo')
            plt.title(str(i), fontsize=18)

        if save_viz_gif:
            dir_out = ('%s/refined_registration_viz' %
                       '/'.join(fname_out.split('/')[:-3]))
            viz_fname_out = '%s/%s_to_%s.gif' % (dir_out, fname0, fname1)
            save_registration_gif(img0, img1, h, viz_fname_out)

        if len(pts0) < min_matches:
            print("Only found %d matches, could not match images %i to %i." %
                   (len(pts0), i, i + 1))
            fname0, img0, kp0, des0 = fname1, img1, kp1, des1
            continue

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

        print('Found %i consistent feature matches between images' %
               len(pts0))

        np.savetxt(fname_out, h.ravel(order='C'))

        # This current homography warps from the previous image to the current
        # one.
        homog_ij[(i - 1, i)] = h

        # Try to reconstruct mapping to previous images by composing
        # homographies. Look at most 10 images back.
        for k in range(1, 10):
            key_desired = (i - k, i)
            try:
                last_h = homog_ij[key_desired]
                last_key = key_desired

                # If the homography is in homog_ij, then it has already been
                # saved.
                continue
            except KeyError:
                pass

            intermediate_key = (i - k, i - k + 1)
            try:
                intermediate_h = homog_ij[intermediate_key]
            except KeyError:
                break

            assert intermediate_key[1] == last_key[0]
            assert key_desired[0] == intermediate_key[0]
            assert key_desired[1] == last_key[1]

            h_desired = np.dot(last_h, intermediate_h)

            # Check to see if there is actually any overlap.
            im_pts = points_along_image_border(image_width, image_height,
                                               num_points=100)
            im_pts = np.vstack([im_pts, np.ones(im_pts.shape[1])])
            im_pts = np.dot(h_desired, im_pts)
            im_pts = im_pts[:2]/im_pts[2]
            ind = np.logical_and(im_pts[0] > 0, im_pts[0] < image_width)
            ind = np.logical_and(ind, im_pts[1] > 0)
            ind = np.logical_and(ind, im_pts[1] < image_height)

            if not np.any(ind):
                break

            homog_ij[key_desired] = h_desired

            # Save this reconstructed homography.
            i1, i2 = key_desired
            fname_from = os.path.splitext(os.path.split(img_fnames[i1])[1])[0]
            fname_to = os.path.splitext(os.path.split(img_fnames[i2])[1])[0]
            fname_base = '%s_to_%s' % (fname_from, fname_to)
            fname_out = '%s/%s.txt' % (homog_out_dir, fname_base)
            np.savetxt(fname_out, h.ravel(order='C'))

            last_h = h_desired
            last_key = key_desired

        fname0, img0, kp0, des0 = fname1, img1, kp1, des1


def stretch_constrast(img):
    img = img.astype(np.float)
    img -= np.percentile(img.ravel(), 0.1)
    img[img < 0] = 0
    img /= np.percentile(img.ravel(), 99.9)/255
    img[img > 225] = 255
    img = np.round(img).astype(np.uint8)

    clahe = cv2.createCLAHE(clipLimit=1, tileGridSize=(5, 5))
    if img.ndim == 3:
        HLS = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
        HLS[:, :, 1] = clahe.apply(HLS[:, :, 1])
        img = cv2.cvtColor(HLS, cv2.COLOR_HLS2BGR)
    else:
        img = clahe.apply(img)

    return img


def save_geotiff(img, camera_model, frame_time, geotiff_fname,
                 compression_setting=None, verbosity=0):
    gdal_drv = gdal.GetDriverByName('GTiff')

    if compression_setting is not None:
        # Compress the RGB and UV.
        gdal_settings = ['COMPRESS=JPEG',
                         'JPEG_QUALITY=%i' % compression_setting]
    else:
        gdal_settings = []

    ds = gdal_drv.Create(geotiff_fname, img.shape[1], img.shape[0], img.ndim,
                         gdal.GDT_Byte, gdal_settings)
    ds.SetProjection(wgs84_cs.ExportToWkt())

    im_pts, corner_ll = get_image_boundary(camera_model, frame_time)
    corner_ll = corner_ll[:, ::-1]

    # Affine transform warping image coordinates to latitude/longitude.
    A = cv2.estimateRigidTransform(np.reshape(im_pts, (1, -1, 2)),
                                   np.reshape(corner_ll, (1, -1, 2)), True)

    if A is None:
        # Failure can happen when the aircraft is just above the ground.
        center = np.mean(corner_ll, axis=0)
        corners = corner_ll - center
        scale = np.max(np.abs(corners))
        corners /= scale

        A = cv2.estimateRigidTransform(np.reshape(im_pts, (1, -1, 2)),
                                       np.reshape(corners, (1, -1, 2)),
                                       True)
        A = np.vstack([A, [0, 0, 1]])
        S = np.identity(3)
        S[0, 0] = S[1, 1] = scale
        A = np.dot(S, A)
        T = np.identity(3)
        T[:2, 2] = center
        A = np.dot(T, A)[:2]

        if False:
            corner_ll2 = np.dot(A, np.vstack([im_pts.T, [1, 1, 1, 1]])).T
            plt.plot(corner_ll.T[0], corner_ll.T[1])
            plt.plot(corner_ll2.T[0], corner_ll2.T[1])

    # Xp = padfTransform[0] + P*padfTransform[1] + L*padfTransform[2]
    # Yp = padfTransform[3] + P*padfTransform[4] + L*padfTransform[5]
    geotrans = [A[0, 2], A[0, 0], A[0, 1], A[1, 2], A[1, 0], A[1, 1]]

    geotrans[1]

    ds.SetGeoTransform(geotrans)

    if ds.RasterCount == 1:
        ds.GetRasterBand(1).WriteArray(img[:, :], 0, 0)
    else:
        for i in range(ds.RasterCount):
            ds.GetRasterBand(i+1).WriteArray(img[:, :, i], 0, 0)

    ds.FlushCache()  # Write to disk.
    ds = None
    if verbosity >= 3:
        print('Saved \'%s\'' % geotiff_fname)


def save_registration_gif(img1, img2, h_1_to_2, fname):
    """
    :param img1: First image.
    :type img1: Numpy array

    :param img2: Second image.
    :type img2: Numpy array

    :param h_1_to_2: Homography that warps img1 coordinates to img2
        coordinates.
    :type h_1_to_2: 3x3 array

    """
    img_rect = cv2.warpPerspective(img1, h_1_to_2, img1.shape[:2][::-1])

    if max(img_rect.shape) > 3000:
        s = 3000/max(img_rect.shape)
        img_rect = cv2.resize(img_rect, None, fx=s, fy=s)
        img2 = cv2.resize(img2, None, fx=s, fy=s)

    images = [Image.fromarray(img2), Image.fromarray(img_rect)]
    images[0].save(fname, format='GIF', append_images=images[1:],
                   save_all=True, duration=300, loop=0)
