## Mutual information minimization on Toy Gaussians

This is a sanity check involving binary classification of bivariate Gaussians. Data is sampled from bivariate normal distributions as follows:
\begin{align}
x_0 &\sim \mathcal{N}\left(\begin{pmatrix} z \\ 0 \end{pmatrix}, \begin{pmatrix} 1 & 0.5 \\ 0.5 & 1 \end{pmatrix}\right) \\
x_1 &\sim \mathcal{N}\left(\begin{pmatrix} 0 \\ 1.5 \end{pmatrix}, \begin{pmatrix} 1 & 0 \\ 0 & 1 \end{pmatrix}\right)
\end{align}
Where $z \sim \mathcal{N}(0,1)$ simulates uncertainty in the $x$-coordinate of samples drawn from the first distribution. The data is then passed through a shallow neural network, which we refer to as the classification network whose objective is to distinguish between samples drawn from each distribution.

To reduce the dependency of the network on the potentially noisy $x$-variable, we penalize an estimate of the mutual information between the logits of the neural network and the $x$-coordinate of the input data. We optimize a lower bound on the mutual information through a shallow neural network of similar capacity to the classification network, which remains unchanged. The degree to which the output encoding of the classification network should be indepedent of the $x$-coordinate can be controlled through the hyperparameter $\lambda$ in the objective function.

```python
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import seaborn as sns
import scipy
from scipy.interpolate import griddata
from sklearn.metrics import roc_curve, auc
plt.style.use('seaborn-darkgrid')
plt.style.use('seaborn-talk')
plt.style.use('seaborn-pastel')
```

```python
import tensorflow as tf
import tensorflow_probability as tfp
import time, os, sys, glob
from lnc import MI
```

    WARNING: The TensorFlow contrib module will not be included in TensorFlow 2.0.
    For more information, please see:
      * https://github.com/tensorflow/community/blob/master/rfcs/20180907-contrib-sunset.md
      * https://github.com/tensorflow/addons
    If you depend on functionality not listed there, please file an issue.

```python
def MINE(x, y, y_prime, training, batch_size, name='MINE', actv=tf.nn.relu, dimension=2, l2_reg=False):
    """
    Mutual Information Neural Estimator
    (x,y):      Drawn from joint
    y_prime:    Drawn from marginal

    returns
    MI:         Lower bound on mutual information between x,y
    """

    init = tf.contrib.layers.xavier_initializer()
    drop_rate = 0.0
    shape = [32,32,32,32]
    kwargs = {'center': True, 'scale': True, 'training': training, 'fused': True, 'renorm': False}

    lambda_l2 = 0.001
    regularization = tf.contrib.layers.l2_regularizer(scale=lambda_l2) if l2_reg else None
    y_prime = tf.random_shuffle(y)

    def statistic_network(t, name='stat_net', reuse=False):
        with tf.variable_scope(name, initializer=init, reuse=reuse) as scope:

            h0 = tf.layers.dense(t, units=shape[0], activation=actv, kernel_regularizer=regularization)
            h1 = tf.layers.dense(h0, units=shape[1], activation=actv, kernel_regularizer=regularization)
            h2 = tf.layers.dense(h1, units=shape[2], activation=actv, kernel_regularizer=regularization)
            out = tf.layers.dense(h2, units=1, kernel_initializer=init, kernel_regularizer=regularization)

        return out

    def log_sum_exp_trick(x, batch_size, axis=1):
        # Compute along batch dimension
        x = tf.squeeze(x)
        x_max = tf.reduce_max(x)
        lse = x_max + tf.log(tf.reduce_sum(tf.exp(x-x_max))) - tf.log(batch_size)
        return lse

    x, y, y_prime = tf.expand_dims(tf.squeeze(x), axis=1), tf.expand_dims(tf.squeeze(y), axis=1), tf.expand_dims(tf.squeeze(y_prime), axis=1)
    z = tf.concat([x,y], axis=1)
    z_prime = tf.concat([x,y_prime], axis=1)
    joint_f = statistic_network(z)
    marginal_f = statistic_network(z_prime, reuse=True)

    print('joint shape', joint_f.shape)
    print('marginal shape', marginal_f.shape)

    MI_lower_bound = tf.squeeze(tf.reduce_mean(joint_f)) - tf.squeeze(log_sum_exp_trick(marginal_f, 1.*batch_size))

    joint_loss = tf.reduce_mean(tf.nn.sigmoid_cross_entropy_with_logits(logits=joint_f,
        labels=tf.ones_like(joint_f)))
    marginal_loss = tf.reduce_mean(tf.nn.sigmoid_cross_entropy_with_logits(logits=marginal_f,
        labels=tf.zeros_like(marginal_f)))

    # Approximate the trained discriminator as the optimal discriminator
    JSD_lower_bound = -(joint_loss + marginal_loss) + tf.log(4.0)

    if args.MI:
        lower_bound = MI_lower_bound
    else:
        lower_bound = JSD_lower_bound

    return lower_bound, joint_f, marginal_f

def dense_network(x, training, name='fully_connected', actv=tf.nn.relu, **kwargs):
    # Toy dense network for binary classification
    init = tf.contrib.layers.xavier_initializer()
    shape = [32, 32, 32, 32, 32]
    kwargs = {'center': True, 'scale': True, 'training': training, 'fused': True, 'renorm': True}

    print('Input X shape', x.get_shape())

    with tf.variable_scope(name, initializer=init, reuse=tf.AUTO_REUSE) as scope:
        h0 = tf.layers.dense(x, units=shape[0], activation=actv)
        # h0 = tf.layers.batch_normalization(h0, **kwargs)
        h1 = tf.layers.dense(h0, units=shape[1], activation=actv)
        # h1 = tf.layers.batch_normalization(h1, **kwargs)
        h2 = tf.layers.dense(h1, units=shape[2], activation=actv)
        # h2 = tf.layers.batch_normalization(h2, **kwargs)

    out = tf.layers.dense(h2, units=1, kernel_initializer=init)

    return out

def mutual_information_1D_kraskov(x, y):
    # k-NN based estimate of mutual information
    from lnc import MI
    mi = MI.mi_LNC([x,y],k=5,base=np.exp(1),alpha=0.5)

    return mi

def scope_variables(name):
    with tf.variable_scope(name):
        return tf.get_collection(tf.GraphKeys.GLOBAL_VARIABLES, scope=tf.get_variable_scope().name)

def plot_decision_surface(sess, model, title='Decision Surface'):
    data, out = [], []

    for _ in range(42):
        data_, out_ = sess.run([model.data, model.softmax])
        data.append(data_)
        out.append(np.squeeze(out_))

    data = np.vstack(data)
    out = np.hstack(out)
    x_grid, y_grid = np.mgrid[-2:3:100j, -2:4:100j]

    z = griddata(points=data[:,:2], values=out, xi=(x_grid, y_grid), method='cubic')
    plt.contourf(x_grid, y_grid, z, 25, vmin=0., vmax=1., cmap=plt.cm.viridis)
    plt.colorbar(label='Output')
    plt.xlabel('$x$')
    plt.ylabel('$y$')
    plt.title(title)
    plt.scatter([0], [0], c="green", linewidths=0, label=r"$\mu_0$")
    plt.scatter([0], [1.5], c="blue", linewidths=0, label=r"$\mu_1$")
    plt.legend(loc="upper left")
    # plt.savefig(os.path.join('results', 'decision_no_pen.pdf'), format='pdf', dpi=256, bbox_inches='tight')
```

```python
class Model():
    def __init__(self, args):

        tfd = tfp.distributions
        self.global_step = tf.Variable(0, trainable=False)
        self.MI_global_step = tf.Variable(0, trainable=False)

        if args.dims == 2:
            dim = 2
            z = tf.random.normal([args.sample_size])
            z_prime = tf.random.shuffle(z)
            mu_x = tf.stack([z, tf.zeros(args.sample_size)], axis=-1)  # [z,0] broadcasted
            mu_y = [0,1.5]

            cov_x = [[1., 0.5], [0.5, 1.]]
            cov_y = [[1., 0.], [0., 1.]]

            standard_mvn_normal = tfd.MultivariateNormalDiag(loc=np.zeros(dim).astype(np.float32))
            scale_x = tf.cholesky(cov_x)
            scale_y = tf.cholesky(cov_y)

            # u ~ N(0,1), v = scale @ u + loc
            self.sample_x = tf.squeeze(tf.matmul(tf.broadcast_to(scale_x, [args.sample_size, dim, dim]),
                tf.expand_dims(standard_mvn_normal.sample(args.sample_size), axis=-1))) + mu_x
            self.sample_y = tf.squeeze(tf.matmul(tf.broadcast_to(scale_y, [args.sample_size, dim, dim]),
                tf.expand_dims(standard_mvn_normal.sample(args.sample_size), axis=-1))) + mu_y

            self.x_data = tf.concat([self.sample_x, tf.zeros([args.sample_size,1])], axis=1)
            self.y_data = tf.concat([self.sample_y, tf.ones([args.sample_size,1])], axis=1)

            data = tf.random.shuffle(tf.concat([self.x_data, self.y_data], axis=0))
            idx = tf.range(tf.shape(data)[0])
            ridx = tf.random.shuffle(idx)[:args.sample_size]
            self.data = tf.gather(data, ridx)
            self.labels = tf.squeeze(self.data[:,2])

        with tf.variable_scope('classifier', reuse=tf.AUTO_REUSE) as scope:
            self.logits = dense_network(x=self.data[:,:2], training=True)

        with tf.variable_scope('miner', reuse=tf.AUTO_REUSE) as scope:
            self.MI_estimate, joint_f, marginal_f = MINE(x=self.logits, y=self.data[:,0], y_prime=tf.random_shuffle(self.data[:,0]),
                batch_size=args.sample_size, dimension=dim, training=True, actv=tf.nn.elu)

        self.softmax = tf.nn.sigmoid(self.logits)
        self.pred = tf.squeeze(tf.cast(tf.greater(self.softmax, 0.5), tf.float32))
        self.accuracy = tf.reduce_mean(tf.cast(tf.equal(self.pred, self.labels), tf.float32))

        self.cross_entropy = tf.nn.sigmoid_cross_entropy_with_logits(logits=self.logits, labels=tf.expand_dims(self.labels,axis=-1))
        self.clf_cost = tf.reduce_mean(self.cross_entropy)
        self.MI_estimate_kNN = tf.py_func(mutual_information_1D_kraskov, inp=[tf.squeeze(self.logits),
            tf.squeeze(self.data[:,0])], Tout=tf.float64)

        # Alternate non-saturating updated rule for generator
        # max_E log(D(E,Z)) = min_E H(D(E,Z),1)
        self.NS_loss = tf.reduce_mean(tf.nn.sigmoid_cross_entropy_with_logits(logits=marginal_f, labels=tf.ones_like(marginal_f)))
        update_ops = tf.get_collection(tf.GraphKeys.UPDATE_OPS)

        if args.penalty:
            if args.alt_update:
                self.cost = self.clf_cost + args.lambda_MI * tf.nn.relu(self.MI_estimate + self.NS_loss)
            else:
                self.cost = self.clf_cost + args.lambda_MI * tf.nn.relu(self.MI_estimate)
        else:
            self.cost = self.clf_cost

        lr = args.lr_E
        theta_f = scope_variables('classifier')
        theta_MINE = scope_variables('miner')
        print('Classifier Parameters', theta_f)
        print('MI-est Parameters', theta_MINE)
        args.optimizer = args.optimizer.lower()
        if args.optimizer == 'adam':
            self.opt = tf.train.AdamOptimizer(lr)
        elif args.optimizer == 'momentum':
            self.opt = tf.train.MomentumOptimizer(lr, momentum=0.9, use_nesterov=True)
        elif args.optimizer == 'rmsprop':
            self.opt = tf.train.RMSPropOptimizer(lr)
        elif args.optimizer == 'sgd':
            self.opt = tf.train.GradientDescentOptimizer(lr)

        with tf.variable_scope('opt', reuse=tf.AUTO_REUSE) as scope:
            self.MI_opt = tf.train.AdamOptimizer(args.lr_T)
            self.grads_clf = self.opt.compute_gradients(self.clf_cost, var_list=theta_f)
            self.grads_mi_clf =  self.opt.compute_gradients(self.MI_estimate, var_list=theta_f)
            self.grads_mi_est = self.MI_opt.compute_gradients(-self.MI_estimate, var_list=theta_MINE)
            self.MINE_opt_op = self.MI_opt.minimize(-self.MI_estimate, global_step=self.MI_global_step,
                    var_list=theta_MINE)

            with tf.control_dependencies(update_ops):
                # Ensures that we execute the update_ops before performing the train_step
                self.opt_op = self.opt.minimize(self.cost, global_step=self.global_step, var_list=theta_f)
```

```python
class args(object):
    rho = 0.5
    sample_size = 512
    dims = 2
    lr_E = 1e-4
    lr_T = 1e-3
    optimizer = 'adam'
    name = 'toy-MI'
    lambda_MI = 10.0
    penalty = False
    MI_iters = 4
    MI = False
    alt_update = False
```

```python
def train(args):

    start_time = time.time()
    ckpt = tf.train.get_checkpoint_state('checkpoints')

    # Build graph
    model = Model(args)
    saver = tf.train.Saver()

    sess = tf.InteractiveSession(config=tf.ConfigProto(allow_soft_placement=True, log_device_placement=False))
    # with tf.Session(config=tf.ConfigProto(allow_soft_placement=True, log_device_placement=False)) as sess:
    sess.run(tf.global_variables_initializer())
    sess.run(tf.local_variables_initializer())
    steps = 5e3

    for step in range(int(steps)):
        sess.run(model.opt_op)

        if args.penalty:
            for MI_step in range(args.MI_iters):
                sess.run(model.MINE_opt_op)

        if step % 128 == 0:
            if args.dims == 2:
                MI_est, MI_kNN, acc, y, y_hat = sess.run([model.MI_estimate, model.MI_estimate_kNN, model.accuracy,
                    model.labels, model.pred])

                print('MINE: {:.3f} | kNN: {:.3f} | Accuracy: {:.3f} | ({:.2f} s)'.format(MI_est,
                    MI_kNN, acc, time.time()-start_time))

    print("Training Complete. Time elapsed: {:.3f} s".format(time.time()-start_time))

    return sess, model
```

```python
sess, model = train(args)
```

    WARNING:tensorflow:From /usr/local/easybuild/software/Tensorflow/1.13.1-intel-2017.u2-GCC-6.2.0-CUDA10-Python-3.6.4-GPU/lib/python3.6/site-packages/tensorflow/python/framework/op_def_library.py:263: colocate_with (from tensorflow.python.framework.ops) is deprecated and will be removed in a future version.
    Instructions for updating:
    Colocations handled automatically by placer.
    Input X shape (512, 2)
    WARNING:tensorflow:From <ipython-input-4-1dba8522a2c9>:72: dense (from tensorflow.python.layers.core) is deprecated and will be removed in a future version.
    Instructions for updating:
    Use keras.layers.dense instead.
    joint shape (512, 1)
    marginal shape (512, 1)
    WARNING:tensorflow:From <ipython-input-5-9bd357dc115b>:51: py_func (from tensorflow.python.ops.script_ops) is deprecated and will be removed in a future version.
    Instructions for updating:
    tf.py_func is deprecated in TF V2. Instead, use
        tf.py_function, which takes a python function which manipulates tf eager
        tensors instead of numpy arrays. It's easy to convert a tf eager tensor to
        an ndarray (just call tensor.numpy()) but having access to eager tensors
        means `tf.py_function`s can use accelerators such as GPUs as well as
        being differentiable using a gradient tape.

    Classifier Parameters [<tf.Variable 'classifier/fully_connected/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/dense/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'classifier/dense/bias:0' shape=(1,) dtype=float32_ref>]
    MI-est Parameters [<tf.Variable 'miner/stat_net/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/bias:0' shape=(1,) dtype=float32_ref>]
    MINE: -0.036 | kNN: 0.645 | Accuracy: 0.385 | (2.29 s)
    MINE: -0.038 | kNN: 0.232 | Accuracy: 0.725 | (2.62 s)
    MINE: -0.037 | kNN: 0.196 | Accuracy: 0.793 | (2.95 s)
    MINE: -0.038 | kNN: 0.142 | Accuracy: 0.783 | (3.29 s)
    MINE: -0.039 | kNN: 0.305 | Accuracy: 0.803 | (3.60 s)
    MINE: -0.035 | kNN: 0.170 | Accuracy: 0.771 | (3.92 s)
    MINE: -0.043 | kNN: 0.238 | Accuracy: 0.793 | (4.25 s)
    MINE: -0.047 | kNN: 0.247 | Accuracy: 0.760 | (4.57 s)
    MINE: -0.044 | kNN: 0.248 | Accuracy: 0.791 | (4.90 s)
    MINE: -0.038 | kNN: 0.184 | Accuracy: 0.783 | (5.22 s)
    MINE: -0.041 | kNN: 0.243 | Accuracy: 0.787 | (5.54 s)
    MINE: -0.041 | kNN: 0.172 | Accuracy: 0.805 | (5.87 s)
    MINE: -0.034 | kNN: 0.159 | Accuracy: 0.795 | (6.19 s)
    MINE: -0.039 | kNN: 0.189 | Accuracy: 0.795 | (6.53 s)
    MINE: -0.040 | kNN: 0.200 | Accuracy: 0.775 | (6.84 s)
    MINE: -0.041 | kNN: 0.184 | Accuracy: 0.787 | (7.17 s)
    MINE: -0.037 | kNN: 0.165 | Accuracy: 0.783 | (7.49 s)
    MINE: -0.037 | kNN: 0.099 | Accuracy: 0.770 | (7.81 s)
    MINE: -0.041 | kNN: 0.238 | Accuracy: 0.803 | (8.13 s)
    MINE: -0.038 | kNN: 0.137 | Accuracy: 0.771 | (8.44 s)
    MINE: -0.051 | kNN: 0.262 | Accuracy: 0.789 | (8.76 s)
    MINE: -0.041 | kNN: 0.190 | Accuracy: 0.812 | (9.09 s)
    MINE: -0.039 | kNN: 0.184 | Accuracy: 0.779 | (9.42 s)
    MINE: -0.039 | kNN: 0.171 | Accuracy: 0.781 | (9.74 s)
    MINE: -0.039 | kNN: 0.149 | Accuracy: 0.775 | (10.06 s)
    MINE: -0.038 | kNN: 0.196 | Accuracy: 0.799 | (10.38 s)
    MINE: -0.042 | kNN: 0.188 | Accuracy: 0.791 | (10.71 s)
    MINE: -0.042 | kNN: 0.196 | Accuracy: 0.805 | (11.03 s)
    MINE: -0.049 | kNN: 0.229 | Accuracy: 0.787 | (11.36 s)
    MINE: -0.041 | kNN: 0.137 | Accuracy: 0.834 | (11.67 s)
    MINE: -0.040 | kNN: 0.136 | Accuracy: 0.805 | (11.99 s)
    MINE: -0.043 | kNN: 0.217 | Accuracy: 0.793 | (12.32 s)
    MINE: -0.038 | kNN: 0.165 | Accuracy: 0.768 | (12.65 s)
    MINE: -0.043 | kNN: 0.109 | Accuracy: 0.766 | (12.97 s)
    MINE: -0.042 | kNN: 0.195 | Accuracy: 0.777 | (13.29 s)
    MINE: -0.037 | kNN: 0.157 | Accuracy: 0.805 | (13.64 s)
    MINE: -0.034 | kNN: 0.193 | Accuracy: 0.760 | (13.96 s)
    MINE: -0.043 | kNN: 0.277 | Accuracy: 0.777 | (14.28 s)
    MINE: -0.043 | kNN: 0.146 | Accuracy: 0.799 | (14.61 s)
    MINE: -0.042 | kNN: 0.219 | Accuracy: 0.814 | (14.92 s)
    Training Complete. Time elapsed: 14.936 s

```python
plot_decision_surface(sess, model, title='Unpenalized Decision Surface')
```

### Switch on MI penalty

```python
sess.close()
args.penalty = True
args.penalty = 1.0
sess_p, model_p = train(args)
```

    Input X shape (512, 2)
    joint shape (512, 1)
    marginal shape (512, 1)
    Classifier Parameters [<tf.Variable 'classifier/fully_connected/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/dense/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'classifier/dense/bias:0' shape=(1,) dtype=float32_ref>]
    MI-est Parameters [<tf.Variable 'miner/stat_net/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/bias:0' shape=(1,) dtype=float32_ref>]
    MINE: -0.003 | kNN: 0.109 | Accuracy: 0.273 | (1.26 s)
    MINE: 0.003 | kNN: 0.225 | Accuracy: 0.664 | (2.94 s)
    MINE: 0.006 | kNN: 0.117 | Accuracy: 0.621 | (4.52 s)
    MINE: 0.002 | kNN: 0.050 | Accuracy: 0.672 | (6.10 s)
    MINE: -0.001 | kNN: 0.033 | Accuracy: 0.711 | (7.69 s)
    MINE: -0.003 | kNN: 0.024 | Accuracy: 0.658 | (9.28 s)
    MINE: 0.002 | kNN: -0.007 | Accuracy: 0.705 | (10.87 s)
    MINE: -0.002 | kNN: 0.091 | Accuracy: 0.688 | (12.45 s)
    MINE: -0.000 | kNN: 0.024 | Accuracy: 0.719 | (14.03 s)
    MINE: 0.002 | kNN: 0.079 | Accuracy: 0.693 | (15.63 s)
    MINE: 0.001 | kNN: 0.042 | Accuracy: 0.746 | (17.20 s)
    MINE: 0.001 | kNN: 0.073 | Accuracy: 0.697 | (18.78 s)
    MINE: 0.003 | kNN: 0.034 | Accuracy: 0.746 | (20.36 s)
    MINE: 0.001 | kNN: 0.022 | Accuracy: 0.750 | (21.94 s)
    MINE: 0.002 | kNN: -0.018 | Accuracy: 0.723 | (23.51 s)
    MINE: 0.000 | kNN: 0.002 | Accuracy: 0.725 | (25.08 s)
    MINE: 0.001 | kNN: 0.023 | Accuracy: 0.771 | (26.65 s)
    MINE: 0.003 | kNN: 0.025 | Accuracy: 0.750 | (28.23 s)
    MINE: -0.000 | kNN: 0.035 | Accuracy: 0.766 | (29.80 s)
    MINE: 0.000 | kNN: 0.041 | Accuracy: 0.756 | (31.39 s)
    MINE: -0.003 | kNN: 0.031 | Accuracy: 0.760 | (32.96 s)
    MINE: 0.003 | kNN: 0.075 | Accuracy: 0.760 | (34.54 s)
    MINE: 0.004 | kNN: 0.024 | Accuracy: 0.785 | (36.11 s)
    MINE: 0.004 | kNN: 0.058 | Accuracy: 0.783 | (37.69 s)
    MINE: 0.003 | kNN: 0.045 | Accuracy: 0.799 | (39.26 s)
    MINE: -0.001 | kNN: 0.044 | Accuracy: 0.768 | (40.80 s)
    MINE: 0.004 | kNN: 0.015 | Accuracy: 0.773 | (42.34 s)
    MINE: 0.002 | kNN: 0.044 | Accuracy: 0.773 | (43.87 s)
    MINE: -0.001 | kNN: 0.023 | Accuracy: 0.762 | (45.41 s)
    MINE: 0.003 | kNN: 0.025 | Accuracy: 0.777 | (46.97 s)
    MINE: 0.002 | kNN: -0.022 | Accuracy: 0.764 | (48.51 s)
    MINE: -0.001 | kNN: 0.057 | Accuracy: 0.801 | (50.06 s)
    MINE: 0.000 | kNN: 0.052 | Accuracy: 0.744 | (51.60 s)
    MINE: 0.000 | kNN: 0.003 | Accuracy: 0.732 | (53.15 s)
    MINE: -0.004 | kNN: 0.045 | Accuracy: 0.756 | (54.69 s)
    MINE: 0.001 | kNN: 0.021 | Accuracy: 0.773 | (56.23 s)
    MINE: 0.003 | kNN: 0.068 | Accuracy: 0.797 | (57.78 s)
    MINE: 0.000 | kNN: 0.017 | Accuracy: 0.754 | (59.32 s)
    MINE: 0.002 | kNN: 0.020 | Accuracy: 0.795 | (60.87 s)
    MINE: -0.000 | kNN: -0.009 | Accuracy: 0.797 | (62.42 s)
    Training Complete. Time elapsed: 62.500 s

```python
plot_decision_surface(sess_p, model_p, title='Penalized Decision Surface')
```

### Turn $\lambda$ up a bit

```python
sess_p.close()
args.penalty = True
args.lambda_MI = 10.0
sess_p, model_p = train(args)
```

    Input X shape (512, 2)
    joint shape (512, 1)
    marginal shape (512, 1)
    Classifier Parameters [<tf.Variable 'classifier/fully_connected/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'classifier/fully_connected/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'classifier/dense/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'classifier/dense/bias:0' shape=(1,) dtype=float32_ref>]
    MI-est Parameters [<tf.Variable 'miner/stat_net/dense/kernel:0' shape=(2, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_1/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/kernel:0' shape=(32, 32) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_2/bias:0' shape=(32,) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/kernel:0' shape=(32, 1) dtype=float32_ref>, <tf.Variable 'miner/stat_net/dense_3/bias:0' shape=(1,) dtype=float32_ref>]
    MINE: 0.008 | kNN: 0.586 | Accuracy: 0.525 | (1.21 s)
    MINE: 0.054 | kNN: 0.183 | Accuracy: 0.496 | (2.77 s)
    MINE: 0.001 | kNN: 0.084 | Accuracy: 0.602 | (4.31 s)
    MINE: -0.000 | kNN: 0.104 | Accuracy: 0.619 | (6.04 s)
    MINE: 0.003 | kNN: 0.072 | Accuracy: 0.623 | (7.59 s)
    MINE: 0.005 | kNN: 0.064 | Accuracy: 0.633 | (9.13 s)
    MINE: -0.004 | kNN: -0.009 | Accuracy: 0.590 | (10.69 s)
    MINE: 0.006 | kNN: 0.107 | Accuracy: 0.641 | (12.23 s)
    MINE: 0.001 | kNN: 0.025 | Accuracy: 0.662 | (13.77 s)
    MINE: 0.002 | kNN: 0.079 | Accuracy: 0.648 | (15.32 s)
    MINE: 0.002 | kNN: 0.006 | Accuracy: 0.660 | (16.88 s)
    MINE: 0.002 | kNN: 0.022 | Accuracy: 0.676 | (18.43 s)
    MINE: -0.001 | kNN: 0.078 | Accuracy: 0.670 | (19.98 s)
    MINE: 0.003 | kNN: 0.024 | Accuracy: 0.697 | (21.53 s)
    MINE: 0.002 | kNN: 0.046 | Accuracy: 0.691 | (23.08 s)
    MINE: 0.001 | kNN: 0.021 | Accuracy: 0.672 | (24.62 s)
    MINE: 0.003 | kNN: 0.088 | Accuracy: 0.652 | (26.16 s)
    MINE: 0.002 | kNN: 0.078 | Accuracy: 0.682 | (27.71 s)
    MINE: -0.001 | kNN: 0.001 | Accuracy: 0.658 | (29.26 s)
    MINE: 0.004 | kNN: 0.057 | Accuracy: 0.680 | (30.81 s)
    MINE: 0.002 | kNN: 0.027 | Accuracy: 0.658 | (32.36 s)
    MINE: -0.003 | kNN: 0.092 | Accuracy: 0.688 | (33.91 s)
    MINE: 0.004 | kNN: 0.065 | Accuracy: 0.689 | (35.46 s)
    MINE: 0.001 | kNN: 0.014 | Accuracy: 0.711 | (37.00 s)
    MINE: 0.003 | kNN: 0.057 | Accuracy: 0.693 | (38.54 s)
    MINE: -0.001 | kNN: 0.055 | Accuracy: 0.719 | (40.07 s)
    MINE: -0.000 | kNN: -0.002 | Accuracy: 0.754 | (41.62 s)
    MINE: 0.004 | kNN: 0.040 | Accuracy: 0.727 | (43.17 s)
    MINE: 0.005 | kNN: 0.070 | Accuracy: 0.703 | (44.70 s)
    MINE: -0.001 | kNN: 0.007 | Accuracy: 0.693 | (46.24 s)
    MINE: 0.002 | kNN: 0.036 | Accuracy: 0.715 | (47.79 s)
    MINE: 0.001 | kNN: 0.081 | Accuracy: 0.717 | (49.33 s)
    MINE: -0.001 | kNN: -0.005 | Accuracy: 0.727 | (50.87 s)
    MINE: 0.001 | kNN: 0.039 | Accuracy: 0.707 | (52.42 s)
    MINE: 0.005 | kNN: 0.033 | Accuracy: 0.723 | (53.96 s)
    MINE: 0.000 | kNN: 0.010 | Accuracy: 0.734 | (55.50 s)
    MINE: -0.002 | kNN: 0.037 | Accuracy: 0.762 | (57.05 s)
    MINE: 0.001 | kNN: 0.040 | Accuracy: 0.740 | (58.59 s)
    MINE: 0.002 | kNN: 0.073 | Accuracy: 0.721 | (60.14 s)
    MINE: 0.000 | kNN: 0.015 | Accuracy: 0.754 | (61.69 s)
    Training Complete. Time elapsed: 61.771 s

```python
plot_decision_surface(sess_p, model_p, title='Penalized Decision Surface, $\lambda=10$')
```


# Data Science Essentials: Applied Optimization

Joaquim Gromicho, 2021

This notebook is part of the module Applied Optimization of the Analytics Academy's Data Science Essentials.

---
 > During this course we make use of Jupyter notebooks hosted by [Google Colab](https://colab.research.google.com/notebooks/intro.ipynb).
  Notebooks deployed on `colab` require neither python nor other dependencies to be installed on your own machine, you only need a browser (preferably `chrome`) and you may also need a google account if you want to execute them.

---

This training consists of a series of self-contained stories.

This whole series makes use of [pyomo](https://en.wikipedia.org/wiki/Pyomo) which is the most complete and versatile Mathematical Optimization package in the python ecosystem.

The notebooks used in this course make extensive use of [markdown](https://en.wikipedia.org/wiki/Markdown) of which [here](https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet) you may find a very comprehensive summary. May you like a document that you can print, fold, and take with you then you may like to check [this](https://enterprise.github.com/downloads/en/markdown-cheatsheet.pdf).

We also draw pictures on our notebooks. Therefore, we need the [magic](https://ipython.readthedocs.io/en/stable/interactive/magics.html) below.

```python
%matplotlib inline
```

For this notebook we need the following packages:
 * sympy
 * math
 * matplotlib
 * IPython
 * pyomo
 * ipopt

If your python distribution is Anaconda, then from the above only the last two are likely not yet to be part of your local python installation.
You need to install those packages on the anaconda prompt. As usual, you should try first with `conda install` and if that does not work then with `pip install`.
You can try the conda distribution as explained in:
https://anaconda.org/conda-forge/pyomo
https://anaconda.org/conda-forge/ipopt
For pyomo please read http://www.pyomo.org/installation and doe ipopt please refer to https://pypi.org/project/ipopt/

## About being pythonic

If you program, then reading [this](https://docs.python-guide.org/writing/style/) will help you writing code that feels _natural_ in python. You may also like to uncomment the following statement for some introspection 😉

```python
# import this
```

## Dependencies

The `import` statement on the cell above is special... it imports programmer's wisdom!
The common usage is to acquire access to python packages.

```python
import sympy
import math
```

---

# Alice: a romantic introduction to Matematical Optimization and to Python

Note: this story was originally told in the book [Optimization](https://press.princeton.edu/books/hardcover/9780691102870/optimization) and the protagonist there is called Samantha, but we stick to the alphabetical order...

As the story goes, Alice receives a beautiful rose.
She has nothing but a lemonade glass to hold the rose and becomes very distressed when the ensemble falls down.

Adding a bit of water helps! Not only that helps the rose, but it also helps the stability: glass, with some water, and rose stands!

Alice thinks: if a bit of water helps, the let us fill the glass!

However, it tilts and falls, as in the beginning, just much more wet.

Alice has a problem to solve: what is the _optimal_ level of water for her rose on a lemonade glass?

She learns from [Archimedes]( https://en.wikipedia.org/wiki/Archimedes) how to compute the _center of gravity_ of the glass with water, which has height
$h = \frac{m_w}{m_w+m_g} h_w + \frac{m_g}{m_w+m_g} h_g$ with:

* $m_w$ the mass of water
* $m_g$ the mass of glass
* $h_w$ the height of the center of gravity of the water in the glass
* $h_g$ the height of the center of gravity of the glass without water

Since Alice's glass is $20$ cm tall, $4$ cm wide and weighs $100$ gram, Alice may fill the glass with water up to height $x$ cm, provided that $0 \leq x \leq 20$ since the water must fit in the glass.

The volume of water is $\pi r^2 x$ with $r$ the radius of the base, i.e. $r=2$.
The volume is therefore $4\pi x$ cubic centimetres.

Since the density of water can be [taken](https://en.wikipedia.org/wiki/Gram_per_cubic_centimetre) as being $1$ gram per cubic centimeter we have:

* $m_w = 4\pi x$
* $m_g = 100$
* $h_w = \frac{x}{2}$
* $h_g = \frac{20}{2} = 10$

And from here we finally obtain the following formula for the height of the center of gravity of the glass with water:

$$
h = \frac{4\pi x}{4\pi x + 100} \frac{x}{2} + \frac{100}{4\pi x + 100} 10 = \frac{4\pi x^2 + 2000}{8\pi x + 200}
$$

Alice's problem is therefore:

$$
\begin{array}{rl}
\min    & \frac{4\pi x^2 + 2000}{8\pi x + 200}  \\
s.t.    & x \geq 0   \\
        & x \leq 20  \\
\end{array}
$$

# Analytical solution

Alice learns from [Fermat]( https://en.wikipedia.org/wiki/Pierre_de_Fermat) that for a function to reach its highest and lowest points inside its domain the derivative must vanish.

This is a good moment to play with symbolic mathematics in python, we will use [sympy](https://www.sympy.org/en/index.html).

## With $\pi$ as a number

```python
# x is a symbol and pi is a number
x  = sympy.Symbol('x')
pi = math.pi

# h is a function of x, and hprime its derivative
h      = (4*pi*x**2 + 2000)/(8*pi*x+200)
hprime = sympy.diff( h, x )

# sol is(are) the value(s) of x that solve hprime(x) == 0
sol = sympy.solveset( hprime, x )
sol
```

$\displaystyle \left\{-22.8735335189926, 6.95803920980307\right\}$

Above we see that the equation $h^\prime(x) = 0$ has two solutions: one negative and one positive.
Obviously, only the positive may be feasible for Alice.
And, since its value is between $0$ and $20$, it is indeed feasible.

You may recall that the sign of the second derivative tells you whether the root of the first derivative is a *maximum*, a *minimum* or a *saddle point*.

```python
opt = max(sol)
sympy.diff( hprime, x ).subs(x,opt).evalf()
```

$\displaystyle 0.0670430626699561$

Since $h^{\prime\prime}(\mbox{opt}) > 0$ it is indeed a (local) **minimum**.

## With $\pi$ as a symbol

```python
# now pi is a symbol, just like x
pi = sympy.Symbol('pi')

# we redefine h using the same right-hand-side code as before,
# but now with x and pi as symbols
h = (4*pi*x**2 + 2000)/(8*pi*x+200)

# to have the drivative on the symbol pi we need it from the new version of h
hprime = sympy.diff( h, x )

solution = sympy.solveset( sympy.diff( h, x ), x )
solution
```

$\displaystyle \left\{- \frac{5 \sqrt{5} \sqrt{4 \pi + 5}}{\pi} - \frac{25}{\pi}, \frac{5 \sqrt{5} \sqrt{4 \pi + 5}}{\pi} - \frac{25}{\pi}\right\} \setminus \left\{- \frac{25}{\pi}\right\}$

Having symbols becomes beneficial if our formulas are displayed in nice mathematical layout.
That is the purpose of the next section.

## Forcing formulas to pretty display using $\LaTeX$

```python
def Preety( formula ):
    from sympy import latex
    from IPython.display import display, Math
    display( Math( latex( formula ) ) )

Preety( h )
Preety( hprime )
Preety( sympy.simplify( hprime ) )
```

$$\frac{4 \pi x^{2} + 2000}{8 \pi x + 200}$$

$$\frac{8 \pi x}{8 \pi x + 200} - \frac{8 \pi \left(4 \pi x^{2} + 2000\right)}{\left(8 \pi x + 200\right)^{2}}$$

$$\frac{\pi \left(- \pi x^{2} + 2 x \left(\pi x + 25\right) - 500\right)}{2 \left(\pi x + 25\right)^{2}}$$

## From a symbolic $\pi$ to a numeric $\pi$

```python
Preety( solution )

s = max(solution.subs( pi, math.pi ).evalf())

print(s)
```

$$\left\{- \frac{5 \sqrt{5} \sqrt{4 \pi + 5}}{\pi} - \frac{25}{\pi}, \frac{5 \sqrt{5} \sqrt{4 \pi + 5}}{\pi} - \frac{25}{\pi}\right\} \setminus \left\{- \frac{25}{\pi}\right\}$$

    6.95803920980307

## A picture says more than thousand words

```python
def Plot( h, s, start, stop, width=18, height=8 ):
    import matplotlib.pyplot as plt
    plt.rcParams["figure.figsize"] = (18,8)

    x = sympy.Symbol('x')
    f = sympy.lambdify( x, h.subs( pi, math.pi ) )

    import numpy
    x = numpy.linspace(start=start,stop=stop,num=100)
    y = f(x)

    plt.plot(x,y,label='$'+sympy.latex(h)+'$',linewidth=3)
    plt.plot(s,f(s), 'ro', label='optimum', markersize=12)

    plt.legend()
    plt.show()
```

```python
Plot( h, s, 0, 20 )
```

# What if we only care about the numerical solution?

## Introducing `pyomo`

This is the moment to meet:
 * mathematical models expressed in `python`, using `pyomo`,
 * powerful numerical optimization algorithms and how to use them.

We now meet [`pyomo`](http://www.pyomo.org/documentation/) which will keep us company for the rest of the course.
Note that in particular you may find this [slide-deck](https://software.sandia.gov/downloads/pub/pyomo/Pyomo-Workshop-Summer-2018.pdf) quite useful.

We will see that `pyomo` completely separates modeling from solving, which allows us to switch solver without recoding!

## Notebook dependencies requiring installation on `colab`

Note that [this notebook](https://nbviewer.jupyter.org/github/jckantor/ND-Pyomo-Cookbook/blob/master/notebooks/01.02-Running-Pyomo-on-Google-Colab.ipynb) explains how to run `Pyomo` on Google Colab.
For a complete overview please check the [cookbook](https://jckantor.github.io/ND-Pyomo-Cookbook/).

```python
import shutil
if not shutil.which('pyomo'):
    !pip install -q pyomo
    assert(shutil.which('pyomo'))
```

         |████████████████████████████████| 9.2 MB 5.1 MB/s
         |████████████████████████████████| 49 kB 4.1 MB/s

```python
from pyomo.environ import ConcreteModel, Var, Objective, minimize
alice     = ConcreteModel('Alice')
alice.h   = Var( bounds=(0,20) )
alice.cog = Objective(expr =(4*math.pi*alice.h**2+2000)/(8*math.pi*alice.h+200)
                     ,sense=minimize)
alice.pprint()
```

    1 Var Declarations
        h : Size=1, Index=None
            Key  : Lower : Value : Upper : Fixed : Stale : Domain
            None :     0 :  None :    20 : False :  True :  Reals

    1 Objective Declarations
        cog : Size=1, Index=None, Active=True
            Key  : Active : Sense    : Expression
            None :   True : minimize : (12.566370614359172*h**2 + 2000)/(25.132741228718345*h + 200)

    2 Declarations: h cog

We may solve the problem even if we have no solver installed!
We simply call a solver on [neos](https://neos-server.org/neos/solvers/index.html) which `pyomo` treats as a _solver manager_.

Note that `neos` requires a valid e-mail address to be specified on the environment variable `NEOS_EMAIL`.

```python
import os
os.environ['NEOS_EMAIL'] = 'you@your.org'
```

```python
from pyomo.environ import SolverManagerFactory
neos = SolverManagerFactory('neos')
%time results = neos.solve( alice, opt='ipopt')
print(results.solver.status, results.solver.termination_condition )
alice.pprint()
```

    CPU times: user 85.7 ms, sys: 19.6 ms, total: 105 ms
    Wall time: 13.7 s
    ok optimal
    1 Var Declarations
        h : Size=1, Index=None
            Key  : Lower : Value            : Upper : Fixed : Stale : Domain
            None :     0 : 6.95803921230998 :    20 : False : False :  Reals

    1 Objective Declarations
        cog : Size=1, Index=None, Active=True
            Key  : Active : Sense    : Expression
            None :   True : minimize : (12.566370614359172*h**2 + 2000)/(25.132741228718345*h + 200)

    2 Declarations: h cog

Besides `pprint`, `pyomo` objects also implement `display`.

```python
alice.display()
```

    Model Alice

      Variables:
        h : Size=1, Index=None
            Key  : Lower : Value            : Upper : Fixed : Stale : Domain
            None :     0 : 6.95803921230998 :    20 : False : False :  Reals

      Objectives:
        cog : Size=1, Index=None, Active=True
            Key  : Active : Value
            None :   True : 6.95803920980307

      Constraints:
        None

The reason why `Alice` reports having no constraints is because we modeled the constraints as `bounds` on the variable.

We may also examine the parts of the model, as the variables, objectives, etc.

```python
alice.h.value
```

    6.95803921230998

```python
alice.cog.expr()
```

    6.95803920980307

It is worth learning more about [magics](https://ipython.readthedocs.io/en/stable/interactive/magics.html), try the magic `%time` above before `solve`.

## What difference does it make if we change solver?

Let us get our own copy of `ipopt`.
We refer again to [this notebook](https://nbviewer.jupyter.org/github/jckantor/ND-Pyomo-Cookbook/blob/master/notebooks/01.02-Running-Pyomo-on-Google-Colab.ipynb) explains how to run `Pyomo` **and how to install solvers** on Google Colab.
For a complete overview please check the [cookbook](https://jckantor.github.io/ND-Pyomo-Cookbook/).

```python
import sys
if 'google.colab' in sys.modules:
    !wget -N -q 'https://ampl.com/dl/open/ipopt/ipopt-linux64.zip'
    !unzip -o -q ipopt-linux64
```

```python
from pyomo.environ import SolverFactory
%time results = SolverFactory('ipopt').solve(alice)
print(results.solver.status, results.solver.termination_condition )
alice.display()
```

    CPU times: user 4.68 ms, sys: 12.7 ms, total: 17.4 ms
    Wall time: 30.7 ms
    ok optimal
    Model Alice

      Variables:
        h : Size=1, Index=None
            Key  : Lower : Value             : Upper : Fixed : Stale : Domain
            None :     0 : 6.958039212309979 :    20 : False : False :  Reals

      Objectives:
        cog : Size=1, Index=None, Active=True
            Key  : Active : Value
            None :   True : 6.95803920980307

      Constraints:
        None

# Conclusion

This notebook shows you how to solve Alice's problem: finding the most stable amount of water in a vase.

You learned how to solve the problem analytically with `sympy`, how to use `matplotlib` to visualize the function and the optimum. And how to model Alice's problem on `pyomo` and solve it with `ipopt` both at [neos](https://neos-server.org/neos/solvers/index.html) and "locally" at your own Colab session.

# Last remarks

This notebook deferred installation of the packages needed to the moment that we actually needed them.

This was deliberate, to show that - for instance - we could solve the problem on `neos` even _before_ having installed our first solver.

Subsequent notebooks will tend to list all dependencies on their top part, which we often call the _preamble_.

```python

```


# We implement an Extended Kalman Filter (EKF) on a double integrator $\ddot{x} = \epsilon$ with range/bearing measurements
We follow https://www.intechopen.com/books/introduction-and-implementations-of-the-kalman-filter/introduction-to-kalman-filter-and-its-applications section 3

### Process Model
\begin{align}
x_{k+1} &= \pmatrix{I_{3 \times 3} & I_{3 \times 3} \Delta t \\ 0_{3 \times 3} & I_{3 \times 3}} x_{k} + v_k \\
   &= Ax_k + v_k
\end{align}

where

$ x_k = [p_k^T, v_k^T] $

$v_k \sim N(0, Q)$

$ Q = \pmatrix{ 0_{3 \times 3} & & 0_{3 \times 3} \\
                                & \sigma_x^2 & 0 & 0 \\
                 0_{3 \times 3} & 0 & \sigma_y^2 & 0 \\
                                & 0 & 0 & \sigma_z^2} $

#### Prediction step
$ \hat{x}_{k+1}^- = A\hat{x}_k^+$

$ P_{k+1}^- = AP_k^+ A^T + Q_m$

where

$Q_m = \pmatrix{ 0_{3 \times 3} & 0_{3 \times 3} \\
                 0_{3 \times 3} & I_{3 \times 3} \sigma_v^2 } $ is the model's Q matrix (note that its different from the real Q here)

### Measurement model

\begin{align}
    z_k = \pmatrix{\phi \\ \theta \\ r} =
    \pmatrix{\text{atan}\left(\frac{x_t-x_s}{y_t - y_s}\right) \\
         \text{atan}\left(\frac{z_t - z_s}{r_{xy}}\right) \\
         r_{xyz}} + w_k
\end{align}

where

$ r_{xy} = \sqrt{(x_t-x_s)^2 + (y_t-y_s)^2} $,
$ r_{xyz} = \sqrt{(x_t-x_s)^2 + (y_t-y_s)^2 + (z_t-y_s)^2} $

$w_k \sim N(0, R)$

$R = \pmatrix{0.02^2 & 0 & 0 \\
              0 & 0.02^2 & 0 \\
              0 & 0 & 1.0^2 } $

#### Update step
$$
H_k = \frac{\partial z_k}{\partial x_k}\Big|_{\hat{x}_k^-} =
   \pmatrix{ \frac{-y}{x^2+y^2} & \frac{x}{x^2+y^2} & 0 \\
             \frac{-xz}{r_{xyz}^2 r_{xy}} & \frac{-yz}{r_{xyz}^2 r_{xy}} & \frac{1}{r_{xy}} & 0_{3 \times 3}\\
             \frac{x}{r_{xyz}} & \frac{y}{r_{xyz}} & \frac{z}{r_{xyz}}}
$$

$ \tilde{y}_k = z_k - h(\hat{x}_k^-)$

$ K_k = P_k^- H_k^T (R_m + H_k P_k^- H_k^T)^{-1} $

$ \hat{x}_k^+ = \hat{x}_k^- + K_k \tilde{y} $

$ P_k^+ = (I - K_k H_k)P_k^- $

where

$R_m = \pmatrix{0.02^2 & 0 & 0 \\
              0 & 0.02^2 & 0 \\
              0 & 0 & 1.0^2 } $ is the model's measurement noise covariance (note here we use the same as the actual measurement's covariance $Q$)

```python
import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
from dymoesco.dynamics.doubleintegrator import DoubleIntegrator
from dymoesco.estimation.filters import EKF
np.set_printoptions(suppress=True, precision=3)
%load_ext autoreload
%autoreload 2
```

```python
# PARAMS
v_std = 0.5
# For now, we set them all equal
x_std = v_std
y_std = v_std
z_std = v_std

x0 = np.array([2,-2,0,5,5.1,0.1])
P0 = np.block([[np.eye(3) * 4**2, np.zeros((3,3))],
               [np.zeros((3,3)), np.eye(3) * 0.4**2]])
a = np.zeros(3)

N = 20
t_span = (0,N)
dt = 1.0
t = np.arange(*t_span, dt)
```

```python
def _g(x):
    return np.linalg.norm(x)
di = DoubleIntegrator(dim=3)
di._g = _g
ddi = di.discretize(1.0)
ddi._g = _g
```

```python
traj = di.simulate(lambda t: a, t_span, x0, t_eval=t, u_std=v_std, y_std=y_std)
```

#### First we write the EKF by calculating the jacobians by hand. Then we'll compare with an autograd implementation

```python
# KEEP THIS!
class HandEKF():

    def __init__(self, x0, P0, p_std, v_std, a_std, dt):
        self.x = x0
        self.P = P0
        self.dt = dt
        self.Q = np.block([[1/4 * dt**4 * np.eye(3), 1/2 * dt**3 * np.eye(3)],
                           [1/2 * dt**3 * np.eye(3), dt**2 * np.eye(3)]])
        self.R = a_std**2 *np.block([[np.eye(3) * p_std**2, np.zeros((3,3))],
                                     [np.zeros((3,3)), np.eye(3) * v_std**2]])
        self.A = np.block([[np.eye(3), np.eye(3)*dt],
                           [np.zeros((3,3)), np.eye(3)]])
        self.B = np.block([[1/2 * np.eye(3) * dt**2],
                           [np.eye(3) * dt]])

    def predict(self, a):
        self.x = self.A @ self.x + self.B @ a
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x

    def update(self, z):
        y = z - self.x
        K = self.P @ np.linalg.pinv(self.R + self.P)
        self.x = self.x + K@y
        self.P = (np.eye(6) - K) @ self.P
        return self.x
```

#### Here we use the autograd EKF

```python
Q = np.block([[np.zeros((3,3)), np.zeros((3,3))],
              [np.zeros((3,3)), v_std * np.eye(3)]])
ekf = EKF(ddi.f, ddi.g, Q, np.eye(1))
```

```python
preds = np.zeros((len(t), 6))
preds[0] = x0
x = x0; P = P0
for i in range(1, len(t)):
    x, P = ekf.predict(x, P, a)
    preds[i] = x
```

```python
# KF with pred + update
Ps = np.zeros((N,) + P0.shape)
Ps[0] = P0
filtered = np.zeros((N,6))
filtered[0] = x0
x = x0; P = P0;
for i in range(1,len(t)):
    x, P = ekf.predict(x, P, a)
    x, P = ekf.update(x, P, traj.y[i])
    filtered[i] = x
    Ps[i] = P
```

```python
from ipywidgets import interact
@interact(ground_truth=True, observations=True, pred=False, pred_and_update=True)
def f(ground_truth, observations, pred, pred_and_update):
    fig, ax = plt.subplots()
    if ground_truth:
        di.plot_phase(traj, label="ground truth", ax=ax, color="black")
    if pred:
        plt.plot(*preds.T, label="KF pred", color="blue")
    #if pred_and_update:
        # TODO: fix this (should only take first 3 dimensions of cov matrix)
        #ekf.plot(filtered, Ps, ax=ax, label="pred+update", color="orange")
    plt.legend()
```

    interactive(children=(Checkbox(value=True, description='ground_truth'), Checkbox(value=True, description='obse…

```python

```


```python
# reference https://socratic.org/questions/the-top-and-bottom-margins-of-a-poster-are-4-cm-and-the-side-margins-are-each-6-
from IPython.display import Image
from IPython.core.display import HTML
from sympy import *; x,h,y,n,t = symbols("x h y n t"); C, D = symbols("C D", real=True)
Image(url= "https://i.imgur.com/LA701Wi.png")
```

```python
A,a,b = symbols("A a b")
A = 382 + 2*(a*8) + 2*(b*8)-4*(8*8)
A
```

$\displaystyle 16 a + 16 b + 126$

```python
Eq(A,a*b)
```

$\displaystyle 16 a + 16 b + 126 = a b$

```python
pprint(solve(Eq(A,a*b),a))  # A(b)
```

    ⎡2⋅(8⋅b + 63)⎤
    ⎢────────────⎥
    ⎣   b - 16   ⎦

```python
simplify((2*(8*b + 63)/(b - 16))*b)   *a * b
```

$\displaystyle \frac{2 b \left(8 b + 63\right)}{b - 16}$

```python
simplify(diff((2*(8*b + 63)/(b - 16))*b))
```

$\displaystyle \frac{16 \left(b^{2} - 32 b - 126\right)}{b^{2} - 32 b + 256}$

```python
print(simplify(diff((2*(8*b + 63)/(b - 16))*b)))
```

    16*(b**2 - 32*b - 126)/(b**2 - 32*b + 256)

```python
b**2 - 32*b - 126
```

$\displaystyle b^{2} - 32 b - 126$

```python
solve(b**2 - 32*b - 126,b)
```

    [16 - sqrt(382), 16 + sqrt(382)]

```python
Image(url= "https://i.imgur.com/3RToGO0.png")
```


# Solver Bare Bones IQ
**author**: Charles-David Hébert

**date**:   January 30 2019

We will solve the Hubbard Model at half-filling, using the known Hartee approximation for the Green Function.

$$ H = \sum_{ij} t_{ ij} c^\dagger_i c_j + U \sum_i n_{i \uparrow} n_{i \downarrow} $$

## Mathematical Description

The project here is to build a dynamical mean field theory solver. Extensive references exists, a good starting point being: [DMFT, A. Georges](https://journals.aps.org/rmp/abstract/10.1103/RevModPhys.68.13).

The basic equations of DMFT are the following (we will stick to two dimensions):

### Math description of DMFT

\begin{align}
G_{imp}(z) & = \int \frac{dk}{(2 \pi)^2} G_{L}(z, k) \\
G_{L}(z, k) & = \bigg[z - \epsilon(k) - \Sigma(z) \bigg]^{-1} \\
G_{imp}(z) & = \bigg[z - \Delta(z) - \Sigma(z) \bigg]^{-1}
\end{align}

### In words description of DMFT
The last three equations are iterated until selfconsistency, let us explain in a bit more detail.

#### The impurity solver

| Input                                 | output                                   |
|---------------------------------------|------------------------------------------|
| Hybridization function := $\Delta(z)$ | Impurity Green function :=  $G_{imp}(z)$ |

The Impurity solver's tasks is to compute the Impurity green function $G_{imp}(z)$, with z being a frequency, here a complex frequency, namely a matsubara frequency : $z := i \omega_n = (2n + 1)\frac{\pi}{\beta} $ .
This can be done in mutiple ways, see the above reference.

#### The selfconsistency scheme

Once the calculation of the impurity green function is done, one must update the hybridization function by using the above equations. Once the new hybridization function has been calculated, we go back to solving the impurity once more. If this is a bit mysterious, don't worry too much, it will get clearer as we progress along.

### Picture description of DMFT

First let us calculate the first moment of the hybridization function;
A high frequency expansion of the above equations yields:

\begin{align}
\Delta(z) & \lim_{z -> \infty} = \frac{\Delta_1}{z} \\
\Delta_1 & =  \int \frac{dk}{(2 \pi)^2} [\epsilon(k)]^2 - \Big[ \int \frac{dk}{(2 \pi)^2} \epsilon(k) \Big]^2  \\
\end{align}

Calculating this high-frequency is important will introduce many concepts

## Introduction to python and functions

```python
import numpy as np
from scipy.integrate import dblquad
```

```python
def eps_k(kx: float, ky:float )-> float:
    """Calculate the dispersion relation. We set the hopping to t = -1.0 as
        the energy unit, and use the 2D square lattice"""
    return -2.0*(np.cos(kx) + np.cos(ky))
```

```python
def get_hyb_fm()-> float:
    # define the limits of the brillouin zone in kx
    kx_limit_low = -np.pi
    kx_limit_high = np.pi

    # the limits of the brillouin zone in ky need to be function, here inline functions without name,
    # also called lambdas
    ky_limit_low = lambda kx: -np.pi
    ky_limit_high = lambda kx: np.pi

    #dblquad returns a tuple with two elements (result: float, error: float),
    # Here, we grab the first element, by indexing with "[0]" to get the result.
    int_eps_k = dblquad(eps_k, kx_limit_low, kx_limit_high, ky_limit_low, ky_limit_high)[0]

    eps_k_squared = lambda kx, ky: eps_k(kx, ky) * eps_k(kx, ky)
    int_eps_k_squared = dblquad(eps_k_squared, kx_limit_low, kx_limit_high, ky_limit_low, ky_limit_high)[0]

    return 1.0/(2.0*np.pi)**2.0*(int_eps_k_squared - int_eps_k)

```

```python
get_hyb_fm()
```

    3.999999999999999

## Introduction to classes

```python
# Let us keep the previous elements and define a model class that will be

class Model:

    def __init__(self, t: float, beta: float, U: float, hyb):
        self.t = t # hopping, usaully set to -1.0
        self.beta = beta # 1/Temperature
        self.U = U # Hubbard interaction
        self.hyb = hyb # hybridization

    def eps_k(self, kx: float, ky:float) -> float:
        return -2.0*(np.cos(kx) + np.cos(ky))

```

```python
from scipy import linalg

class ImpuritySolver:

    def __init__(self, model: Model):
        self.model = model

    def solve(self):
        green_impurity = np.zeros(self.model.hyb.shape, dtype=complex)

        # We know the result for the Hartee part of the self-energy:
        # Sigma(z) = U/2.0

        self_energy = self.model.U / 2.0 *np.ones(green_impurity.shape, dtype=complex)

        for n in range(green_impurity.shape[0]):
            iwn = 1.0j * (2.0 * n + 1.0 ) * np.pi / self.model.beta

            # we index the numpy array with the "[]" operator. If the array was two dimensional,
            # we could index with two variables.
            green_impurity[n] = linalg.inv(iwn - self.model.hyb[n] - self_energy[n])

        return green_impurity

```

```python
class SelfConsistency:

    def __init__(self, model, green_impurity):
        self.model = model
        self.green_impurity = green_impurity

    def green_lattice_scalar(kx, ky, self_energy, n):
        """ Return the lattice green function for one frequency, given by the index n"""

        iwn = 1.0j * (2.0 * n + 1.0) * np.pi / self.model.beta
        return linalg.inv(iwn - model.eps_k(kx, ky) - self_energy[n])

    def run_selfconsistency():

        # 0.) extract the self-energy from the green_impurity

        self_energy = np.zeros(green_impurity.shape)
        for n in range(green_impurity.shape[0]):
            iwn = 1.0j * (2.0 * n + 1.0 ) * np.pi / self.model.beta
            self_energy[n] = iwn - self.model.hyb[n] - linalg.inv(green_impurity[n])

        # 1.) Calculate a new green_impurity using the lattice green function
        green_impurity_new = np.zeros(self.green_impurity.shape, dtype=complex)

        kx_limit_low = -np.pi
        kx_limit_high = np.pi
        ky_limit_low = lambda kx: -np.pi
        ky_limit_high = lambda kx: np.pi

        for n in range(green_impurity_new.shape[0]):
            green_impurity_new[n] = dblquad(green_lattice, kx_limit_low, kx_limit_high, ky_limit_low, ky_limit_high,
                                           args=(self_energy, n))[0]

        # 2.) extract new hyb
        hyb_new = np.zeros(self.green_impurity.shape, dtype=complex)
        for n in range(hyb_new.shape[0]):
            hyb_new[n] = linalg.inv(iwn - green_impurity_new[n] - self_energy[n])

        return hyb_new
```

```python

def main():

    # 0.) define the simulation parameters
    n_freq: int = 2 # we keep 100 frequencies
    t = -1.0
    beta = 10.0
    U = 4.0
    iter_max = 2 # maximum number of iterations

    # initial guess for the hybridization, hmmm. no clue so put it to zero !
    hyb = np.zeros(n_freq, dtype=complex)

    for ii in range(iter_max):
        model = Model(t, beta, U, hyb)
        impurity_solver = ImpuritySolver(model)
        green_impurity = impurity_solver.solve()
        selfconsistency = SelfConsistency(model, green_impurity)
        hyb = selfconsistency.run_selfconsistency()
        print("ii = ", ii)

    print(hyb)
    print(2)
```

```python
main()
```

```python

```


Diner Robot Inverse Kinematics calculations

```python
import sympy as sp
import numpy as np
import math

```

UR5 angles and link lengths:

```python
q1, q2, q3, q4, q5, q6, q7, = sp.symbols('q1 q2 q3 q4 q5 q6 q7')
d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]
a = [0.0, -0.425, -0.39225, 0.0, 0.0, 0]

```

Transformation Matrix:

```python
T0 = sp.Matrix([[1, 0, 0, 0],
               [0, 1, 0, 0],
               [0, 0, 1, 0],
               [0, 0, 0, 1]])

T1 = sp.Matrix([[sp.cos(q1), 0, -sp.sin(q1), 0],
                [sp.sin(q1),     0, sp.cos(q1), 0],
                [0,             -1,   0,       d[0]],
                [0, 0, 0, 1]])

#T1 = T1.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T2 = sp.Matrix([[sp.cos(q2), 0, sp.sin(q2), 0],
                [sp.sin(q2), 0, -sp.cos(q2), 0],
                [0, 1, 0, d[1]],
                [0, 0, 0, 1]])

#T2 = T2.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})
q3 = 0
T3 = sp.Matrix([[sp.cos(q3), 0, sp.sin(q3), 0],
                [sp.sin(q3), 0, -sp.cos(q3), 0],
                [0, 1, 0, d[2]],
                [0, 0, 0, 1]])

#T3 = T3.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T4 = sp.Matrix([[sp.cos(q4), 0, -sp.sin(q4), 0],
                [sp.sin(q4), 0, sp.cos(q4), 0],
                [0, -1, 0, d[3]],
                [0, 0, 0, 1]])
#T4 = T4.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T5 = sp.Matrix([[sp.cos(q5), 0, -sp.sin(q5), 0],
                [sp.sin(q5), 0, sp.cos(q5), 0],
                [0, -1, 0, d[4]],
                [0, 0, 0, 1]])

#T5 = T5.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

T6 = sp.Matrix([[sp.cos(q6), 0, sp.sin(q6), 0],
                [sp.sin(q6), 0, -sp.cos(q6), 0],
                [0, 1, 0, d[5]],
                [0, 0, 0, 1]])
#T6= T6.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})

```

Final Transformation Matrix:

```python
T = (T0*T1*T2*T3*T4*T5*T6)

T
```

$\displaystyle \left[\begin{matrix}\left(\left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} & - \left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} & \left(\left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} - \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)}\right) \sin{\left(q_{6} \right)} - \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{6} \right)} & - 0.0823 \left(\sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} - 0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \sin{\left(q_{1} \right)} + 0.09465 \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{4} \right)} - 0.09465 \sin{\left(q_{4} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\\\left(\left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} & - \left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} & \left(\left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{6} \right)} - \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{6} \right)} & - 0.0823 \left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{5} \right)} + 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} - 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} - 0.10915 \cos{\left(q_{1} \right)}\\\left(- \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} - \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \sin{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{5} \right)} \cos{\left(q_{6} \right)} & - \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{5} \right)} & - \left(- \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} - \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)}\right) \cos{\left(q_{6} \right)} + \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{6} \right)} \cos{\left(q_{5} \right)} & - 0.0823 \left(- \sin{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + \sin{\left(q_{4} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{5} \right)} + 0.09465 \sin{\left(q_{2} \right)} \sin{\left(q_{4} \right)} + 0.09465 \cos{\left(q_{2} \right)} \cos{\left(q_{4} \right)} + 0.089159\\0 & 0 & 0 & 1\end{matrix}\right]$

Transformation at start position:

```python
q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])
Tstart = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
Tstart
```

$\displaystyle \left[\begin{matrix}-1.74523786614818 \cdot 10^{-5} & -0.999999999695393 & -1.74535971126676 \cdot 10^{-5} & 0.0268483480543087\\-5.23601821401489 \cdot 10^{-5} & -1.74526832816749 \cdot 10^{-5} & 0.999999998476908 & 0.0946504685989643\\-0.999999998476913 & 1.7453292508424 \cdot 10^{-5} & -5.23598775333623 \cdot 10^{-5} & 0.0891571324977001\\0 & 0 & 0 & 1\end{matrix}\right]$

Jacobian calculation:

```python
J = sp.Matrix([[-a[1]*sp.sin(q1)*sp.sin(q2) - d[5]*sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - a[2]*sp.sin(q1)*sp.sin(q2 + q3) + d[4]*sp.sin(q1)*sp.sin(q2 + q3 + q4) + d[5]*sp.cos(q1)*sp.cos(q5) + d[3]*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -
                  a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *
                  sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +
                  d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),
                - d[5]*sp.sin(q1)*sp.sin(q5) + d[5]*sp.cos(q1) *
                sp.cos(q5)*sp.cos(q2 + q3 + q4),
                0],
               [d[5]*sp.sin(q1)*sp.cos(q5) + d[3]*sp.sin(q1) + a[1]*sp.sin(q2)*sp.cos(q1) + d[5]*sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4) + a[2]*sp.sin(q2 + q3)*sp.cos(q1) - d[4]*sp.sin(q2 + q3 + q4)*sp.cos(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -
                   a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *
                   sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +
                   d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),
                d[5]*sp.sin(q1)*sp.cos(q5)*sp.cos(q2 + q3 + q4) +
                d[5]*sp.sin(q5)*sp.cos(q1),
                0],
               [0,
                a[1]*sp.sin(q2) + d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) +
                a[2]*sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) + a[2] *
                sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) -
                d[4]*sp.sin(q2 + q3 + q4),
                d[5]*sp.sin(q2 + q3 + q4)*sp.cos(q5),
                0],
               [0, sp.sin(q1), sp.sin(q1), sp.sin(q1), -sp.sin(q2 + q3 + q4)*sp.cos(q1),
                sp.sin(q1)*sp.cos(q5) + sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4)],
               [0, -sp.cos(q1), -sp.cos(q1), -sp.cos(q1), -sp.sin(q1)*sp.sin(q2 + q3 + q4),
                sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - sp.cos(q1)*sp.cos(q5)],
               [1, 0, 0, 0, sp.cos(q2 + q3 + q4), sp.sin(q5)*sp.sin(q2 + q3 + q4)]])
J

```

$\displaystyle \left[\begin{matrix}0.81725 \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} - 0.0823 \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} + 0.09465 \sin{\left(q_{1} \right)} \sin{\left(q_{2} + q_{4} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.81725 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.39225 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \cos{\left(q_{1} \right)} & - 0.0823 \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} + 0.0823 \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} & 0\\0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + 0.10915 \sin{\left(q_{1} \right)} - 0.81725 \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.81725 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.39225 \cos{\left(q_{2} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & \left(- 0.0823 \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)} - 0.09465 \cos{\left(q_{2} + q_{4} \right)}\right) \sin{\left(q_{1} \right)} & 0.0823 \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} & 0\\0 & - 0.81725 \sin{\left(q_{2} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & - 0.39225 \sin{\left(q_{2} \right)} + 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & 0.0823 \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - 0.09465 \sin{\left(q_{2} + q_{4} \right)} & 0.0823 \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{5} \right)} & 0\\0 & \sin{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} & - \sin{\left(q_{2} + q_{4} \right)} \cos{\left(q_{1} \right)} & \sin{\left(q_{1} \right)} \cos{\left(q_{5} \right)} + \sin{\left(q_{5} \right)} \cos{\left(q_{1} \right)} \cos{\left(q_{2} + q_{4} \right)}\\0 & - \cos{\left(q_{1} \right)} & - \cos{\left(q_{1} \right)} & - \cos{\left(q_{1} \right)} & - \sin{\left(q_{1} \right)} \sin{\left(q_{2} + q_{4} \right)} & \sin{\left(q_{1} \right)} \sin{\left(q_{5} \right)} \cos{\left(q_{2} + q_{4} \right)} - \cos{\left(q_{1} \right)} \cos{\left(q_{5} \right)}\\1 & 0 & 0 & 0 & \cos{\left(q_{2} + q_{4} \right)} & \sin{\left(q_{5} \right)} \sin{\left(q_{2} + q_{4} \right)}\end{matrix}\right]$

```python
q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])
Jstart = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
Jstart
```

$\displaystyle \left[\begin{matrix}-0.0946390777151275 & 1.42636782391076 \cdot 10^{-5} & 6.84602891958906 \cdot 10^{-6} & -2.50700136463318 \cdot 10^{-11} & -1.43640597409963 \cdot 10^{-6} & 0\\0.191448348253117 & -0.817248563345077 & -0.39224856347454 & 1.43640597411485 \cdot 10^{-6} & -2.50700268809156 \cdot 10^{-11} & 0\\0 & 0.0946357362966888 & 0.0946431539460094 & 0.09465 & -0.082299999987465 & 0\\0 & 0.999999999847691 & 0.999999999847691 & 0.999999999847691 & -1.74532925191725 \cdot 10^{-5} & 0.999999999695383\\0 & 1.74532925191725 \cdot 10^{-5} & 1.74532925191725 \cdot 10^{-5} & 1.74532925191725 \cdot 10^{-5} & 0.999999999847691 & 1.74532925165142 \cdot 10^{-5}\\1 & 0 & 0 & 0 & -1.60812264967664 \cdot 10^{-16} & -1.74532925190572 \cdot 10^{-5}\end{matrix}\right]$

Waypoint Generation:

```python
qdash = sp.Matrix([ [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)]])

theta = np.pi/2

while(theta< ((2*np.pi) + np.pi/2)):
    Js = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5]})
    try:
        Jp = Js.inv()
    except:
        print('Singularity occured')
        q = (q + qdash).evalf()
    radius = 100
    Vx = -radius*np.sin(theta)*((2*np.pi) / 40)
    Vz = radius*np.cos(theta)*((2*np.pi) / 40)
    theta += ((2*np.pi) / 40)
    V = sp.Matrix([ [Vx], [0], [Vz], [0], [0], [0]])
    qdash=Jp*V
    #q_current= q_pervious + 𝑞̇_current . ∆t
    q = (q + qdash).evalf()

    Tdash = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })
    x = Tdash[0,3]
    y = Tdash[1,3]
    z = Tdash[2,3]

    print('x: ',x)
    print('y: ',y)
    print('z: ',z)

```

    x:  -0.103148396181722
    y:  0.0438657283681490
    z:  -0.0355430285281103
    x:  -0.147273159898797
    y:  -0.125202044444935
    z:  0.124921686787584
    x:  -0.00937470520408906
    y:  -0.176525091404733
    z:  0.173067649289900
    x:  0.0427741715230540
    y:  0.0457480001443346
    z:  0.174259377166235
    x:  0.200543121647522
    y:  -0.0465204867667080
    z:  0.0942175473649006
    x:  0.169376367062934
    y:  -0.0628805749009145
    z:  0.147405750141163
    x:  -0.134108848018656
    y:  0.0477309380107116
    z:  0.0328496368472096
    x:  -0.0350128115402672
    y:  0.0515125302498191
    z:  0.00857264081972968
    x:  -0.169606940063976
    y:  0.0988007497385818
    z:  0.151133368225458
    x:  -0.184433570610797
    y:  -0.0372919938820578
    z:  0.186481385745066
    x:  -0.00990999729662447
    y:  0.0445627947231336
    z:  0.177971844273570
    x:  -0.0576029004372159
    y:  0.0709841622164546
    z:  -0.0310210247623603
    Singularity occured
    x:  0.203005119143047
    y:  0.0201969079554846
    z:  0.152017052475800
    x:  0.160648802671862
    y:  -0.0530220562046413
    z:  0.0914412222132482
    x:  0.0709230323404108
    y:  0.0714487921069014
    z:  0.0752101830647717
    x:  0.113419298694141
    y:  -0.138450091962684
    z:  -0.0138227579807484
    x:  -0.0662967586403884
    y:  -0.0668554221950254
    z:  0.00915276648810031
    x:  -0.199541201193674
    y:  -0.0354925315615094
    z:  0.0787001938399681
    x:  -0.123104025540629
    y:  0.0115586624282927
    z:  0.173154659364518
    x:  -0.0814403328514375
    y:  0.162513388345019
    z:  0.185558652419942
    x:  0.0380996583327256
    y:  0.0679197985395505
    z:  0.149309033042676
    x:  -0.0174403172925617
    y:  0.0869443544770498
    z:  -0.0319437852086548
    x:  -0.100645009355833
    y:  0.0332332432424140
    z:  -0.0324276269241449
    x:  -0.0969571742203594
    y:  0.0103086517259070
    z:  0.200678604211980
    x:  0.0590625064303628
    y:  -0.0451298281083800
    z:  -0.00742153409214925
    x:  -0.0407356020896451
    y:  0.0883687577773600
    z:  0.126692415459500
    x:  0.0703878408935651
    y:  0.0663175685738039
    z:  -0.00434934606126991
    x:  0.0158311061762567
    y:  0.0459512835742991
    z:  0.192073463732889
    Singularity occured
    x:  -0.147150931015434
    y:  0.139781190636149
    z:  0.0372244168588145
    x:  -0.0466785404697651
    y:  0.200460967122675
    z:  0.145376416304960
    x:  -0.150464368091746
    y:  0.0192014133460974
    z:  0.0338198542996504
    x:  0.0143818348683777
    y:  -0.0770862493933570
    z:  -0.0300539329563644
    Singularity occured
    x:  0.174641032076904
    y:  0.0894126858472976
    z:  0.00479471558306991
    x:  -0.103326895483028
    y:  -0.160937358613615
    z:  0.0211206718056824
    x:  0.0885162493401619
    y:  0.180639467472392
    z:  0.134417659979678
    x:  -0.0165738006154281
    y:  0.0488425167188685
    z:  0.180814713126384
    x:  0.126874745384624
    y:  -0.0159131656002903
    z:  0.0325312652304365
    x:  -0.156766615937550
    y:  -0.0590423776609432
    z:  0.192240684272611
    x:  0.0513729551442988
    y:  -0.0448545243770389
    z:  -0.00971169175926069
    x:  0.0503098007065950
    y:  -0.101772739030657
    z:  -0.0299103754424827


# Correspondence

Solution to find matching triangles between two meshes
from the paper:

- Robert W. Sumner, Jovan Popovic. Deformation Transfer for Triangle Meshes. ACM Transactions on Graphics. 23, 3. August 2004.
    http://graphics.csail.mit.edu/~sumner/research/deftransfer/

Task:

Find a triangle mapping of a source mesh to a target mesh.

Idea:

Repeated closest point transformations controlled by user-defined markers

## Algorithm:
1. Transform the source mesh to match user-defined markers while keeping the identity.
2. Search for closest vertices in the target.
3. Transform the source mesh again but additionally with the closest points.
4. Repeat from step 2.

The transformations in each step start from the initial source mesh.
It is updated iteratively by the transformation
that is created by the least squares solution of cost functions.
Instead for searching for a transformation, the costs are solved directly for the transformed mesh.
The user-defined markers forces the equation system
to solve for the target shape.
The smoothness cost $E_S$ and identity cost $E_I$ try to preserve the source mesh.
By enforcing the markers these cost functions find a "best" fit for the source
into the target mesh.
The closest point cost $E_C$ is the third cost function which
weighting is increase iteratively and so it slowly adapts the target mesh.
The closest points are re-computed after each iteration.
In the first iteration there are no closest points used (since there are none yet).

## Triangle transformation

A triangle (or face) without position can be described in the form of span vectors written in columns
that are produced by the three vertices $\vec{a}, \vec{b}, \vec{c}$ and a fourth square-rooted normal $\vec{d}$.
The vertex $\vec{d}$ allows lateral movement of the face $V_i$ when a transformation is applied.

$$V_i = \left[\vec{b}-\vec{a}, \vec{c}-\vec{a}, \vec{d}-\vec{a}\right]$$

The fourth span vector $\vec{d}$ is the square-root normalized normal of the face, given by:

\begin{equation}
    \vec{d} = \vec{a} + \frac{\left(\vec{b}-\vec{a}\right)\times\left(\vec{c}-\vec{a}\right)}
    {\sqrt{\left|\left(\vec{b}-\vec{a}\right)\times\left(\vec{c}-\vec{a}\right)\right|}}
\end{equation}

A face (or triangle) transformation $T_i$ for a face-index $i$ is then given by:

$$\widetilde{V}_i = T_i\cdot V_i \longrightarrow T_i=\widetilde{V}_i V_i^{-1}$$

## Transformation Definition

The transformation to the target mesh is controlled by a cost function
that can be solved directly for the vertices of the transformed mesh including the fourth face normal.
It searches for the least square solution for the vertices $x = (\vec{v}_1, ..., \vec{v}_n)$, $x \in \mathbb{R}^{3 \times n}$ which contains
the vertices of the mesh as well as the artifical fourth face normals.

The solution can be expressed as the sum of the cost functions weighted by $w_S$, $w_I$, and $w_C$.

\begin{equation}
    \widetilde{x} = \underset{\widetilde{V}}{\min}\left( w_S E_S + w_I  E_I + w_C E_C \right)
\end{equation}

Name | Equation
:---- | :-----
Smoothness | $$E_S(v_1,...,v_n)=\sum_{T_i}\sum_{T_j\in\text{adj}(T_i)}\left\|T_i - T_j\right\|^2_F$$
Identity | $$ E_I(v_1,...,v_n) = \sum_{T_i}\left\|T_i-\mathbf{I}\right\|^2_F $$
Closest Valid Point | $$E_C(v_1,...,v_n; c_1,...,c_n)=\sum_{i}\left\|v_i-c_i\right\|^2$$

Each cost function can be expanded and expressed as a sum of individual terms
and merged back into a single Frobenius norm (like an accordion).

For example the identity cost can be converted as followed:

## Frobenius Norm
#### Merging equations

The equations can be merged into a single Frobenius Norm which can be solved by common solvers.

\begin{align}
\sum_{k}\left\|Z_k\right\|^2_F &= \sum_{k}z_{k1}^2+z_{k2}^2+...+z_{kl}^2
\\
 &= (z_{11}^2+z_{12}^2+...+z_{1l}^2)+ (z_{21}^2+z_{22}^2+...+z_{2l}^2) + ... + (z_{k1}^2+z_{k2}^2+...+z_{kl}^2)
 \\
 &= \left\|\sum_{k}Z_k\right\|^2_F
\end{align}

with $l$ element in $Z$.

#### Transposed

The frobenius norm and its minimization does not change if the equation is transposed.

\begin{equation}
\left\|Z\right\|^2_F = z_{1}^2+z_{2}^2+...+z_{l}^2 = \left\|Z_k^T\right\|^2_F
\end{equation}

## Transformation Structure

A equation system of squared Frobenius norms can be reordered into another structure.
This can be used to structure a matrix equation system that can be solved efficiently.

### Matrix Structure
The triangle transformation $T_i$ can be expressed as a equation on all vertices $x \in \mathbb{R}^{3\times n}$
and $\hat{V}^{-1} \in \mathbb{R}^{n\times 3}$:

\begin{equation}
T_i = \widetilde{V} V^{-1} \quad \xrightarrow[]{\text{Expand to } x} \quad \hat{T}_i = x \hat{V}^{-1}
\end{equation}

This applied to any $T_i$, it follows with $C_i \in \mathbb{R}^{3\times 3}$ as constant:

\begin{align}
\left\|T_i-C_i\right\|^2_F
&= \left\|\widetilde{V}_i V_i^{-1}-C_i\right\|^2_F
& \xrightarrow[]{\text{Expand to } x} \quad& \left\|x \hat{V}_i^{-1}-C_i\right\|^2_F
\end{align}

Transposing and merging the equations for $m$ triangle transformations $T_i$

\begin{align}
\sum_{i=1}^{m}\left\|x\hat{V}^{-1}-C_i\right\|^2_F
&= \sum_{i=1}^{m}\left\| \left( x\hat{V}^{-1}-C_i \right)^T \right\|^2_F
\\&=\sum_{i=1}^{m}\left\| \left(\hat{V}_i^{-1}\right)^T x^T - C_i^T  \right\|^2_F
\\&= \left\| \begin{pmatrix}
\left(\hat{V}_1^{-1}\right)^T \\
\left(\hat{V}_2^{-1}\right)^T \\
... \\
\left(\hat{V}_m^{-1}\right)^T
\end{pmatrix}
x^T - \begin{pmatrix}
C_1^T \\
C_2^T \\
... \\
C_m^T
\end{pmatrix} \right\|^2_F
&&= \left\|Ax^T-b\right\|^2_F
\end{align}

### Cost Matrices

Each cost builds a separate matrix in which each row is minimized with a least square method.

#### Identity cost matrix

\begin{equation}
E_I(v_1,...,v_n) = \sum_m^{\left|T_i\right|}\left\|T_i-\mathbf{I}\right\|^2_F
= \left\| \begin{pmatrix}
\left(\hat{V}_1^{-1}\right)^T \\
\left(\hat{V}_2^{-1}\right)^T \\
... \\
\left(\hat{V}_m^{-1}\right)^T
\end{pmatrix}
x^T - \begin{pmatrix}
\mathbf{I} \\
\mathbf{I} \\
... \\
\mathbf{I}
\end{pmatrix} \right\|^2_F
= \left\|A_Ix^T-b_I\right\|^2_F
\end{equation}

where $A_I \in \mathbb{R}^{3m\times n}$  and $b_i \in \mathbb{R}^{3m\times 3}$

#### Smoothness cost matrix

\begin{equation}
E_S(v_1,...,v_n)=\sum_m^{\left|T_i\right|}\sum_{T_j\in\text{adj}(T_i)}\left\|T_i - T_j\right\|^2_F
= \left\| \begin{pmatrix}
\left(\hat{V}_1^{-1} - \hat{V}_{j_1}^{-1}\right)^T \\
\left(\hat{V}_1^{-1} - \hat{V}_{j_2}^{-1}\right)^T \\
... \\
\left(\hat{V}_1^{-1} - \hat{V}_{j_{\left|\text{adj}(T_i)\right|}}^{-1}\right)^T \\
\left(\hat{V}_2^{-1} - \hat{V}_{j_1}^{-1}\right)^T \\
... \\
\end{pmatrix}
x^T - 0
\right\|^2_F
= \left\|A_Sx^T-b_S\right\|^2_F
\end{equation}

where $A_S \in \mathbb{R}^{3q\times n}$, $b_i \in \mathbb{R}^{3q\times 3}$, and
$q = \sum_m^{\left|T_i\right|}\left|\text{adj}(T_i)\right|$.

### Closest point cost matrix

The closest point is the 2-Norm and can be handled the same way like the Frobenius norm.

\begin{equation}
E_C(v_1,...,v_n; c_1,...,c_n)=\sum_{i}\left\|v_i-c_i\right\|^2
= \left\| \mathbf{I}
x^T - \begin{pmatrix}
c_1^T \\
c_2^T \\
... \\
c_n^T
\end{pmatrix}\right\|^2_F
= \left\|A_Cx^T-b_C\right\|^2_F
\end{equation}

where $A_C \in \mathbb{R}^{n\times n}$  is the identity, and $b_c \in \mathbb{R}^{n\times 3}$ are the transposed
closes points.

### Final equation for least-squares solver
Each cost is weighted, which needs to be reflected into the equation system.
We can do this by weighting each row.

\begin{equation}
E = w_S E_S + w_I  E_I + w_C E_C
\end{equation}

\begin{align}
\widetilde{x} = \underset{x}{\text{arg}\min}\left\|
\begin{pmatrix}
w_I A_I \\
w_S A_S \\
w_C A_C \\
\end{pmatrix}x^T -
\begin{pmatrix}
w_I b_I \\
w_S b_S \\
w_C b_C \\
\end{pmatrix}
\right\|^2_F
\end{align}

Before the least-squares solution is searched, the columns for all marker vertices
are resolved and subtracted from the right side $b_I, b_S, b_C$.
Then the marker positions on the left side $A_I, A_S, A_C$ are set to 0.
This forces the equation to find a solution with the marker vertices fixed.

### Transformation Vertex Indicies
In the expansion of V to $\hat{V}$ for the big $x$ vertices list each transformation is indexed as followed:

\begin{align}
    \mathbf{T} &= \widetilde{V}V^{-1}
    = \left[\vec{b}-\vec{a}, \vec{c}-\vec{a}, \vec{d}-\vec{a}\right] V^{-1}  \\
    \\
    &= \begin{pmatrix}
        b_{1} - a_{1} & c_{1} - a_{1} & d_{1} - a_{1} \\
        b_{2} - a_{2} & c_{2} - a_{2} & d_{2} - a_{2} \\
        b_{3} - a_{3} & c_{3} - a_{3} & d_{3} - a_{3}
    \end{pmatrix}
    \begin{pmatrix}
        \overline{v}_{1,1} & \overline{v}_{1,2} & \overline{v}_{1,3} \\
        \overline{v}_{2,1} & \overline{v}_{2,2} & \overline{v}_{2,3} \\
        \overline{v}_{3,1} & \overline{v}_{3,2} & \overline{v}_{3,3}
    \end{pmatrix}
\end{align}

The resulting matrix $\mathbb{R}^{3\times 3}$ has the following equation for each entry:

\begin{align}
    w_{i,j}
    &= (b_{i} - a_{i}) \overline{v}_{1,j}
    + (c_{i} - a_{i}) \overline{v}_{2,j}
    + (d_{i} - a_{i}) \overline{v}_{3,j}
    \\
    &= b_{i}\overline{v}_{1,j}  - a_{i}\overline{v}_{1,j}
    + c_{i}\overline{v}_{2,j} - a_{i}\overline{v}_{2,j}
    + d_{i}\overline{v}_{3,j} - a_{i}\overline{v}_{3,j}
    \\
    &=
    - (\overline{v}_{1,j} + \overline{v}_{2,j}+ \overline{v}_{3,j})a_{i}
    +\overline{v}_{1,j}b_{i}
    + \overline{v}_{2,j}c_{i}
    + \overline{v}_{3,j}d_{i}
\end{align}

## Implemenation

Numpy + Scipy + Plotly (for plotting)

### Optimization

Since the three XYZ components of the vertices are independent, the minimization problem can be solved independetly
by component. One optimization is to create a diagonal block matrix and solve it single-threaded;
or use multiple threads to solve the least-square problem for each XYZ component.

### Imports

```python

```