hyperellipsoid.py

# Copyright 2020 Jannis Harder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
# REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
# AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
# INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
# LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
# OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
# PERFORMANCE OF THIS SOFTWARE.

import numpy as np
import scipy.linalg.lapack as lapack
import scipy.optimize as optimize


def hyperellipsoid_nearest(W, y):
    """
    find x with x^T W x = 1 minimizing |x-y|_2
    where W is a positive definite matrix.
    """

    # AFAICT the scipy/numpy wrappers for eigenvalues all call more general
    # lapack routines than required here, so we're using dsyev directly.
    w, Q, info = lapack.dsyev(W)

    if info != 0:
        raise RuntimeError('error computing eigenvectors')

    w = w[::-1]
    Q = Q[:, ::-1]

    xc = cartesian_hyperellipsoid_nearest(w, y @ Q)

    return xc @ Q.T


def cartesian_hyperellipsoid_nearest(w, y):
    """
    find x with x^T W x = 1 minimizing |x-y|_2
    where W is a diagonal matrix with diagonal w.
    w must be positive and decreasing.
    """

    x = np.zeros(len(w), dtype=w.dtype)

    if np.all(y == 0):
        x[0] = 1 / w[0]
        return x

    sr = y * w @ y
    if sr == 1:
        x[:] = y
        return x

    skip = 0
    while skip < len(y) and y[skip] == 0:
        skip += 1
        if sr < 1:
            i = skip - 1
            if w[skip] < w[i]:
                x[skip:] = y[skip:] / (1 - w[skip:] / w[i])
                xi2 = (1 - np.sum(x[skip:]**2 * w[skip:])) / w[i]
                if xi2 >= 0:
                    x[i] = np.sqrt(xi2)
                    return x

    w_in, y_in = w, y
    w = w[skip:]
    y = y[skip:]

    d = 1 - w / w[0]

    def f(delta):
        if delta == 0:
            return -1
        else:
            return 1 / np.sum(w * (y / (delta * w + d))**2) - 1

    if sr < 1:
        lo = 0
        hi = 1 / w[0]
    else:
        lo = 1 / w[0]
        hi = 2 / w[0]

        while f(hi) < 0:
            hi *= 2

    delta = optimize.root_scalar(f, method='toms748', bracket=(lo, hi))

    if not delta.converged:
        raise RuntimeError('root_scalar did not converge')

    delta = delta.root

    x[skip:] = y / (delta * w + d)

    return x


if __name__ == '__main__':
    # A small 2-dimensional demo
    import matplotlib.pyplot as plt

    tiles_x = 3
    tiles_y = 3

    for i in range(tiles_x * tiles_y):
        plt.subplot(tiles_x, tiles_y, 1 + i)

        W = np.random.randn(2, 2)
        W = W @ W.T + np.identity(2) * 0.4

        x_0 = np.arange(-4, 4, 0.01)
        x_1 = np.arange(-4, 4, 0.01)

        x = x_0[:, None, None] * [1, 0] + x_1[None, :, None] * [0, 1]

        f = ((x @ W)[..., None, :] @ x[..., :, None] - 1)[..., 0, 0]

        plt.gca().set_aspect(1)
        plt.gca().set_yticklabels([])
        plt.gca().set_xticklabels([])
        plt.ylim(-4, 4)
        plt.xlim(-4, 4)

        plt.contour(x_0, x_1, f.T, [0])

        for i in range(20):
            y = np.random.randn(2) * np.random.rand() * 2
            x = hyperellipsoid_nearest(W, y)

            plt.plot([y[0], x[0]], [y[1], x[1]])

    plt.tight_layout()
    plt.show()