# Expectation Maximization Algorithm

In this article, exctation maximization(EM) algorithm will be introduced. EM algorithm is introduced as early as 1950 by Ceppellini etal. in the context of gene frequency estimation, the expectation maximization algorithm was nanalyzed more generally by Hartley and by Baum et al. In this article, I will begin my story with an example of flipping coins and maximum likelihood estimator(MLE). This example is from the paper [what is the expectation maximization algorithm?](https://www.nature.com/articles/nbt1406)

## 1.1 Example of flipping coins

Suppose we have two coins A and B with unknown biases, $\theta_{A}$ and $\theta_{B}$ respectively. We do the experiemt that we fip the coins repeatedly. During the experiment, we keep track the record of the $x={x_{1},x_{2},...,x_{n}}$ and $z={z_{1},z_{2},...,z_{n}}$, where $x$ indicates the number of heads we observe when we fip the coin and $z$ imply the identity of coin we use to fip. Given $x$ and $z$, we want to estimated biases $\theta$ of these two coins. Obviously, we can use MLE to estimate the biase $\theta$ based on the bays theorem:

$$\begin{equation*}
p(\theta \vert x, z)=\frac{p(\theta)p(x,z\vert \theta)}{p(x,z)}
\end{equation*}$$

where we assume the normalization term and prior term are constant, thus to maximize the posterior is equivalent to maximize the likelihood $p(x,z\vert \theta)$. That's a **commom parameter estimation problem.** This kind of estimated problem is called parameter estimated with complete data.

Now, we consider a more challenging variant problem that we still we want to estimate baises of two coins A and B, but in this time, we only has the record $x$ and do not know about the $z$, where $z$ is called hidden parameter or latent factor. Parameter estimated problem with complete data is converted into parameter estimated problem with imcomplete data, so the MLE is not effective for the problem of imcomplete data. This is a big limitation of MLE and thus we need another method to sovle for imcomplete data problem.

However, if we have some way to know the value of $z$, we can use MLE to inference $\theta$ which is the same as parameter estimation with complete data. Thus, we can reduce parameter estimation for this problem with incomplete data to maximum likelihood estimation with complete data.

One iterative scheme for obtaining completions caould works as follows: starting from some initial parameters, $\widetilde{\theta}^{(t)}=(\widetilde{\theta}_{A}^{(t)},\widetilde{\theta}_{B}^{(t)})$, determine for each of the five sets whether coin $A$ or coin $B$ was more liekly to have generated the observed flips(using the current parameter estimates). Then, assume these completions (that is , guessed coin assignments) to be correct, and apply the regular maximum likelihood estimation procedure to get $\widetilde{\theta}^{(t+1)}$. Finally, repeat these two steps until convergence.

The expectation maximization algorithm is a refinement on this basic idea that the expectation maximization algorithm computes probabilities for each possible completion of the missing data, using the current parameters $\widetilde{\theta}^{(t)}$. These probabilities are used to create a weighted training set consisting of all possible completions of the data. Then, a modified version of maximum likelihood estimation that deals with weighted training examples provides new parameter estimates, $\widetilde{\theta}^{(t+1)}$.

The figure below show the procedure of maximum likelihood estimator and expectation maximization algorithm. The figure is from the paper:[ what is the expectation maximization algorithm?](https://www.nature.com/articles/nbt1406)

### Summary

**Intuitively, the expectation maximization algorithm alternates between the steps of guessing a probability distribution over completions of missing data given the current model( known as the E-step). and then re-estimating the model parameters using these completions(known as the M-step).**

## 1.2 Mathematical foundations(Convergence at intuitive level)

In the complete data case, the objective function $\log(P(x,z\vert \theta))$ has a single global optimum, which can often be found in closed form. In contrast, in the incomplete data case the function $\log(P(x\vert \theta))$ has multiple local maximum and no closed form solution.

To deal with this, the expectation maximization algorithm reduces the difficult task of optimization $\log(P(x\vert \theta))$ into a sequence of simpler optimization subproblems, whose objective functions have unique global maxima that can often be computed in closed form. These subproblems are chosen in a way that gurantees their corresponding solutions $\widetilde{\theta}^{(1)},\widetilde{\theta}^{(2)},...$ and will converge to a local optimmum of $\log(P(x\vert \theta))$.

More specifically, the expectation maximization algorithm alternates between two phases. During the E-step, expectation maximization chooses a function $g_{t}$, that lower bounds $\log(P(x\vert \theta))$ everywhere, and for which $g_{t}(\widetilde{\theta}^{(t)}=\log(P(x\vert \widetilde{\theta}^{(t)}))$. During the M-step, the expectation maximization algorithm moves to a new parameter set $\widetilde{\theta}^{(t+1)}$ that maximizes $g_{t}$. As the value of the lower-bound $g_{t}$ matches the objective function at $\widetilde{\theta}^{(t)}$, it follows that $\log(P(x\vert \widetilde{\theta}^{(t)}))=g_{t}(\widetilde{\theta}^{(t)})\leq g_{t}(\widetilde{\theta}^{(t+1)})=\log(P(x\vert \widetilde{\theta}^{(t+1)}))$. So the objective function monotonically increases during each iteration of expectation maximization.

## 1.3 Jenson's inequality

Jenson's inequality states that for convex function $f$, if $\lambda \in [0,1]$, then

$$
\begin{equation*}
\lambda f(x_{1})+(1-\lambda)f(x_{2}) \geq f(\lambda x_{1}+(1-\lambda)x_{2})
\end{equation*}
$$

Gerenrally, the formula above can be rewritten as:

$$
\begin{equation*}
\mathbb{E}[f(x)]\geq f(\mathbb{E}[x])
\end{equation*}
$$

The physical meaning is shown as below:

### EM Algorithm and Jensen's inequality

We re-write the complete log likelihood function by multiplication it by $\frac{q(z)}{q(z)}$, where $q(z)$ represents an arbitrary distribution for the random variable $Z$.

$$
\begin{align*}
\log p(x\vert \theta) &= \log \sum_{Z}p(z\vert \theta)p(x\vert z,\theta)\frac{q(z)}{q(z)} \\
&= \log \mathbb{E}_{q}\bigg[\frac{p(z\vert \theta)p(x\vert z, \theta)}{q(z)}\bigg] \\
&\geq \mathbb{E}_{q}\bigg[\log \frac{p(z\vert \theta)p(x\vert z, \theta)}{q(z)}\bigg] \\
\end{align*}
$$

**Note that this function is concave function, so the symbol of inequality changes the direction**,

let' s $\mathcal{L}(\theta \vert q)= \mathbb{E}_{q}\bigg[\log \frac{p(z\vert \theta)p(x\vert z, \theta)}{q(z)}\bigg]$, then

$$
\begin{equation}
\mathcal{L}(\theta \vert q) = \mathbb{E}_{q}[\log p(z\vert \theta)]+\mathbb{E}_{q}[\log p(x\vert z,\theta)] -\mathbb{E}_{q}[\log q(z)]
\end{equation}
$$

This is the objective function of EM algorithm. Note that the third term in the equation is known as entropy.

## 1.4 The EM algorithm

The EM algorithm proceeds by coordinate ascent. At each iteration $t$, we have update two parameters: $q^{(t)}$ and $\theta^{(t)}$.

At the E-step: we update the posterior value $q$ of the random variable given the observations while holding $\theta^{(t)}$ fixed.

$$
\begin{align*}
q^{(t+1)} &=
\arg\max_{q} \mathcal{L}(q,\theta^{(t)}) \\
&= p(z\vert x, \theta^{(t)})
\end{align*}
$$

At the M-step, we update the model parameters to maximize the expected complete log likelihood function.

$$
\begin{equation}
\theta^{(t+1)} = \arg\max_{\theta} \mathcal{L}(q^{(t+1)},\theta)
\end{equation}
$$

In detail, let's see how it works:

$$
\begin{align*}
\mathcal{L}(p(z\vert x, \theta),\theta) &= \sum_{Z}p(z\vert x, \theta) \log \frac{p(x,z\vert \theta)}{p(z\vert x, \theta)} \\
&= \sum_{Z}p(z\vert x, \theta)\log \frac{p(x,z\vert \theta)p(x\vert \theta)}{p(x,z\vert \theta)} \\
&= \sum_{Z}p(z\vert x, \theta)\log p(x\vert \theta) \\
&= \log p(x\vert \theta)\sum_{Z}p(x\vert x, \theta) \\
&= \log p(x\vert \theta)
\end{align*}
$$

Therefore, as we maximize the objective function, we are also maximizing the log likelihood function.

## There some codes about coin toss example(these examples are from [sta663](https://people.duke.edu/~ccc14/sta-663/EMAlgorithm.html))

```python
import os
import sys
import glob
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
%matplotlib inline
plt.style.use('ggplot')
np.random.seed(1234)

np.set_printoptions(formatter={'all':lambda x: '%.3f' % x})

from IPython.display import Image
from numpy.core.umath_tests import matrix_multiply as mm

from scipy.optimize import minimize
from scipy.stats import bernoulli, binom
```

## Solving for complete likelihood

```python
def neg_log_likelihood(theta, n, xs, zs):
    return -np.sum([binom(n, theta[z]).logpmf(x) for (x, z) in zip(xs, zs)])
```

```python
# each time we flip the coin repeatedly 10 times
num = 10

theta_A = 0.8 # the baise of coin A
theta_B = 0.4 # the baise of coin B

coin_A = bernoulli(theta_A)
coin_B = bernoulli(theta_B)

xs = np.array(list(map(sum, [coin_A.rvs(num), coin_B.rvs(num), coin_A.rvs(num), coin_A.rvs(num), coin_B.rvs(num)])))
zs = [0,1,0,0,1]
```

### Sovle by hand

```python
ml_A = np.sum(xs[[0,2,3]])/(3.0*num)
ml_B = np.sum(xs[[1,4]])/(2.0*num)

print(ml_A, ml_B)
```

    0.833333333333 0.45

### Solve by iterative algorithm

```python
bounds = [(0,1),(0,1)]
minimize(neg_log_likelihood, (0.5, 0.5), args=(num,xs,zs), method='tnc',
        bounds=bounds, options={'maxiter': 100})
```

         fun: 8.7695482351748506
         jac: array([-0.000, -0.000])
     message: 'Converged (|f_n-f_(n-1)| ~= 0)'
        nfev: 24
         nit: 7
      status: 1
     success: True
           x: array([0.833, 0.450])

## Solve for incomplete likelihood

```python
# we only know the data xs, and we need to guess the thetas firstly.
xs = np.array([(5,5), (9,1), (8,2), (4,6), (7,3)])
thetas = np.array([[0.6, 0.4], [0.5, 0.5]])

tol = 0.01 # stop criterion
num_maxiter = 100
ll_old = 0 #LogLikeliHood
for i in range(num_maxiter):
    ws_A = []
    ws_B = []

    vs_A = []
    vs_B = []

    ll_new=0
    for x in xs:
        # E-step: compute the probability distribution over all possible completions
        ll_A = np.sum([x*np.log(thetas[0])])
        ll_B = np.sum([x*np.log(thetas[1])])

        demo = np.exp(ll_A)+np.exp(ll_B)
        w_A = np.exp(ll_A)/demo
        w_B = np.exp(ll_B)/demo

        ws_A.append(w_A)
        ws_B.append(w_B)

        v_A = np.dot(w_A, x)
        v_B = np.dot(w_B, x)

        vs_A.append(v_A)
        vs_B.append(v_B)
        ll_new += w_A*ll_A +w_B*ll_B

    # M-step: update the parameter of theta based on the current model
    thetas[0] = np.sum(vs_A, 0)/np.sum(vs_A)
    thetas[1] = np.sum(vs_B, 0)/np.sum(vs_B)

    print('the iteration is %d'% (i+1))
    print('theta_A=%.2f, theta_B=%.2f, ll=%.2f' % (thetas[0,0], thetas[1,0], ll_new))

    if np.abs(ll_new-ll_old)<tol:
        break
    ll_old=ll_new
```

    the iteration is 1
    theta_A=0.71, theta_B=0.58, ll=-32.69
    the iteration is 2
    theta_A=0.75, theta_B=0.57, ll=-31.26
    the iteration is 3
    theta_A=0.77, theta_B=0.55, ll=-30.76
    the iteration is 4
    theta_A=0.78, theta_B=0.53, ll=-30.33
    the iteration is 5
    theta_A=0.79, theta_B=0.53, ll=-30.07
    the iteration is 6
    theta_A=0.79, theta_B=0.52, ll=-29.95
    the iteration is 7
    theta_A=0.80, theta_B=0.52, ll=-29.90
    the iteration is 8
    theta_A=0.80, theta_B=0.52, ll=-29.88
    the iteration is 9
    theta_A=0.80, theta_B=0.52, ll=-29.87

```python
# we only know the data xs, and we need to guess the thetas firstly.
xs = np.array([(5,5), (9,1), (8,2), (4,6), (7,3)])
thetas = np.array([[0.6, 0.4], [0.5, 0.5]])

tol = 0.01 # stop criterion
num_maxiter = 100
ll_old = -np.infty #LogLikeliHood
for i in range(num_maxiter):
    ws_A = []
    ws_B = []

    vs_A = []
    vs_B = []

    ll_new=0
    for x in xs:
        # E-step: compute the probability distribution over all possible completions
        ll_A = np.sum([x*np.log(thetas[0])])
        ll_B = np.sum([x*np.log(thetas[1])])

        demo = np.exp(ll_A)+np.exp(ll_B)
        w_A = np.exp(ll_A)/demo
        w_B = np.exp(ll_B)/demo

        ws_A.append(w_A)
        ws_B.append(w_B)

        v_A = np.dot(w_A, x)
        v_B = np.dot(w_B, x)

        vs_A.append(v_A)
        vs_B.append(v_B)
        ll_new += w_A*ll_A +w_B*ll_B

    # M-step: update the parameter of theta based on the current model
    thetas[0] = np.sum(vs_A, 0)/np.sum(vs_A)
    thetas[1] = np.sum(vs_B, 0)/np.sum(vs_B)

    print('the iteration is %d'% (i+1))
    print('theta_A=%.2f, theta_B=%.2f, ll=%.2f' % (thetas[0,0], thetas[1,0], ll_new))

    if np.abs(ll_new-ll_old)<tol:
        break
    ll_old=ll_new
```

    the iteration is 1
    theta_A=0.71, theta_B=0.58, ll=-32.69
    the iteration is 2
    theta_A=0.75, theta_B=0.57, ll=-31.26
    the iteration is 3
    theta_A=0.77, theta_B=0.55, ll=-30.76
    the iteration is 4
    theta_A=0.78, theta_B=0.53, ll=-30.33
    the iteration is 5
    theta_A=0.79, theta_B=0.53, ll=-30.07
    the iteration is 6
    theta_A=0.79, theta_B=0.52, ll=-29.95
    the iteration is 7
    theta_A=0.80, theta_B=0.52, ll=-29.90
    the iteration is 8
    theta_A=0.80, theta_B=0.52, ll=-29.88
    the iteration is 9
    theta_A=0.80, theta_B=0.52, ll=-29.87

```python
# we only know the data xs, and we need to guess the thetas firstly.
xs = np.array([(5,5), (9,1), (8,2), (4,6), (7,3)])
thetas = np.array([[0.6, 0.4], [0.5, 0.5]])

tol = 0.01 # stop criterion
num_maxiter = 100
ll_old = -np.infty #LogLikeliHood
for i in range(num_maxiter):
    # E-step: compute the probability distribution over all possible completions
    ll_A = np.sum(xs*np.log(thetas[0]), axis=1)
    ll_B = np.sum(xs*np.log(thetas[1]), axis=1)

    demo = np.exp(ll_A)+np.exp(ll_B)
    w_A = np.exp(ll_A)/demo
    w_B = np.exp(ll_B)/demo

    vs_A = w_A[:, None] * xs
    vs_B = w_B[:, None] * xs
    # M-step: update the parameter of theta based on the current model
    thetas[0] = np.sum(vs_A, 0)/np.sum(vs_A)
    thetas[1] = np.sum(vs_B, 0)/np.sum(vs_B)

    ll_new = w_A.dot(ll_A) + w_B.dot(ll_B)-w_A.dot(np.log(w_A))-w_B.dot(np.log(w_B))

    print('the iteration is %d'% (i+1))
    print('theta_A=%.3f, theta_B=%.3f, ll=%.3f' % (thetas[0,0], thetas[1,0], ll_new))

    if np.abs(ll_new-ll_old)<tol:
        break
    ll_old=ll_new
```

    the iteration is 1
    theta_A=0.713, theta_B=0.581, ll=-29.628
    the iteration is 2
    theta_A=0.745, theta_B=0.569, ll=-28.394
    the iteration is 3
    theta_A=0.768, theta_B=0.550, ll=-28.257
    the iteration is 4
    theta_A=0.783, theta_B=0.535, ll=-28.162
    the iteration is 5
    theta_A=0.791, theta_B=0.526, ll=-28.119
    the iteration is 6
    theta_A=0.795, theta_B=0.522, ll=-28.107
    the iteration is 7
    theta_A=0.796, theta_B=0.521, ll=-28.105

```python
def em(xs, thetas, maxiter=100, tol=1e-6):
    '''Expectation-maximization for coin sample problem'''

    ll_old = -np.infty
    for i in range(maxiter):
        ll = np.array([np.sum(xs * np.log(theta), axis=1) for theta in thetas])
        lik = np.exp(ll)
        ws = lik/lik.sum(0)
        vs = np.array([w[:,None] * xs for w in ws])
        thetas = np.array([v.sum(0)/v.sum() for v in vs])
        ll_new = np.sum([w*l for w, l in zip(ws, ll)])
        if np.abs(ll_new - ll_old) < tol:
            break
        ll_old = ll_new
    return i, thetas, ll_new
```

```python
# we only know the data xs, and we need to guess the thetas firstly.
xs = np.array([(5,5), (9,1), (8,2), (4,6), (7,3)])
thetas = np.array([[0.6, 0.4], [0.5, 0.5]])

i, thetas, ll = em(xs, thetas)
print(i)
for theta in thetas:
    print(theta)
print(ll)
```

    18
    [0.797 0.203]
    [0.520 0.480]
    -29.868676155

```python
np.random.seed(1234)

n = 100
p0 = 0.8
p1 = 0.35

xs = np.concatenate([np.random.binomial(n, p0, int(n/2)), np.random.binomial(n, p1, int(n/2))])
xs = np.column_stack([xs, n-xs])
np.random.shuffle(xs)
```

```python
results = [em(xs, np.random.random((2,2))) for i in range(10)]
i, thetas, ll = sorted(results, key=lambda x: x[-1])[-1]
print(i)
for theta in thetas:
    print(theta)
print(ll)
```

    4
    [0.352 0.648]
    [0.798 0.202]
    -5756.59565198

```python
import scipy.stats as st

def f(x,y):
    z = np.column_stack([x.ravel(), y.ravel()])
    return (0.1*st.multivariate_normal([0,0], 1*np.eye(2)).pdf(z) +
           0.4*st.multivariate_normal([3,3], 2*np.eye(2)).pdf(z) +
           0.5*st.multivariate_normal([0,5], 3*np.eye(2)).pdf(z))
```

```python
f(np.arange(3), np.arange(3))
```

    array([0.017, 0.012, 0.023])

```python
s = 200
x = np.linspace(-3, 6, s)
y = np.linspace(-3, 8, s)
X, Y = np.meshgrid(x,y)
Z = np.reshape(f(X,Y),(s,s))

from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure(figsize=(12,8))
ax = fig.add_subplot(111, projection='3d')
ax.plot_surface(X, Y, Z, cmap='jet')
plt.title('Gaussian Mixure, Model')
```

```python
from scipy.stats import multivariate_normal as mvn
```

```python
def em_gmm_orig(xs, pis, mus, sigmas, tol=0.01, max_iter=100):
    n, p = xs.shape
    k = len(pis)

    ll_old = 0
    for i in range(max_iter):
        exp_A = []
        exp_B = []
        ll_new = 0

        # E-step
        ws = np.zeros((k, n))
        for j in range(len(mus)):
            for i in range(n):
                ws[j, i] = pis[j] * mvn(mus[j], sigmas[j]).pdf(xs[i])
        ws /= ws.sum(0)

        # M-step
        pis = np.zeros(k)
        for j in range(len(mus)):
            for i in range(n):
                pis[j] += ws[j, i]
        pis /= n

        mus = np.zeros((k,p))
        for j in range(k):
            for i in range(n):
                mus[j] += ws[j, i] * xs[i]
            mus[j] /= ws[j, :].sum()

        sigmas = np.zeros((k,p,p))
        for j in range(k):
            for i in range(n):
                ys = np.reshape(xs[i]-mus[j],(2,1))
                sigmas[j] += ws[j,i] * np.dot(ys, ys.T)
            sigmas[j] /= ws[j,:].sum()

        #update complete log likelihood
        ll_new = 0.0
        for i in range(n):
            s = 0
            for j in range(k):
                s += pis[j] * mvn(mus[j], sigmas[j]).pdf(xs[i])
            ll_new += np.log(s)

        if np.abs(ll_new - ll_old) < tol:
            break
        ll_old = ll_new

    return ll_new, pis, mus, sigmas
```

### EM algorithm (Vector version)

```python
def em_gmm_vect(xs, pis, mus, sigmas, tol=0.01, max_iter=100):

    n, p = xs.shape
    k = len(pis)

    ll_old = 0
    for i in range(max_iter):
        exp_A = []
        exp_B = []
        ll_new = 0

        # E-step
        ws = np.zeros((k,n))
        for j in range(k):
            ws[j,:] = pis[j] * mvn(mus[j], sigmas[j]).pdf(xs)
        ws /= ws.sum(0)

        # M-step
        pis = ws.sum(axis=1)
        pis /= n

        mus = np.dot(ws, xs)
        mus /= ws.sum(1)[:, None]

        sigmas = np.zeros((k, p, p))
        for j in range(k):
            ys = xs - mus[j,:]
            sigmas[j] = (ws[j, :, None, None] * mm(ys[:,:,None], ys[:,None, :])).sum(axis=0)
        sigmas /= ws.sum(axis=1)[:,None,None]

        # update complete log likelihood
        ll_new = 0
        for pi, mu, sigma in zip(pis, mus, sigmas):
            ll_new += pi*mvn(mu, sigma).pdf(xs)
        ll_new = np.log(ll_new).sum()

        if np.abs(ll_new - ll_old) < tol:
            break
        ll_old = ll_new

    return ll_new, pis, mus, sigmas
```

### EM algorithm (Einstein summation)

```python
def em_gmm_eins(xs, pis, mus, sigmas, tol=0.01, max_iter=100):

    n, p = xs.shape
    k = len(pis)

    ll_old = 0
    for i in range(max_iter):
        exp_A = []
        exp_B = []
        ll_new = 0

        # E-step
        ws = np.zeros((k, n))
        for j, (pi, mu, sigma) in enumerate(zip(pis, mus, sigmas)):
            ws[j, :] = pi * mvn(mu, sigma).pdf(xs)
        ws /= ws.sum(0)

        # M-step
        pis = np.einsum('kn->k', ws)/n
        mus = np.einsum('kn,np -> kp', ws, xs)/ws.sum(1)[:, None]
        sigmas = np.einsum('kn,knp,knq -> kpq', ws,
            xs-mus[:,None,:], xs-mus[:,None,:])/ws.sum(axis=1)[:,None,None]

        # update complete log likelihoood
        ll_new = 0
        for pi, mu, sigma in zip(pis, mus, sigmas):
            ll_new += pi*mvn(mu, sigma).pdf(xs)
        ll_new = np.log(ll_new).sum()

        if np.abs(ll_new - ll_old) < tol:
            break
        ll_old = ll_new

    return ll_new, pis, mus, sigmas
```

```python

```

```python
np.random.seed(123)

# create data set
n = 1000
_mus = np.array([[0,4], [-2,0]])
_sigmas = np.array([[[3,0], [0,0.5]],[[1,0],[0,2]]])
_pis = np.array([0.6, 0.4])
xs = np.concatenate([np.random.multivariate_normal(mu, sigma, int(pi*n))
                     for pi, mu, sigma in zip(_pis, _mus, _sigmas)])

# initial guesses for paramters
pis = np.random.random(2)
pis /= pis.sum()
mus = np.random.random((2,2))
sigmas = np.array([np.eye(2)] * 2)
```

```python
%%time
ll1, pis1, mus1, sigmas1 = em_gmm_orig(xs, pis, mus, sigmas)
```

    CPU times: user 9.27 s, sys: 107 ms, total: 9.38 s
    Wall time: 9.4 s

```python
intervals = 101
ys = np.linspace(-8, 8, intervals)
X, Y = np.meshgrid(ys, ys)
_ys = np.vstack([X.ravel(), Y.ravel()]).T

z = np.zeros(len(_ys))
for pi, mu, sigma in zip(pis1, mus1, sigmas1):
    z += pi*mvn(mu, sigma).pdf(_ys)
z = z.reshape((intervals, intervals))

plt.figure(figsize=(10, 8))
ax = plt.subplot(111)
plt.scatter(xs[:,0], xs[:,1], alpha=0.2)
plt.contour(X, Y, z, N=10)
plt.axis([-8, 6, -6,8])
ax.axes.set_aspect('equal')
plt.tight_layout()
```

```python
%%time
ll2, pis2, mus2, sigmas2 = em_gmm_vect(xs, pis, mus, sigmas)
```

    CPU times: user 19.7 ms, sys: 3.68 ms, total: 23.4 ms
    Wall time: 21.1 ms

```python
intervals = 101
ys = np.linspace(-8, 8, intervals)
X, Y = np.meshgrid(ys, ys)
_ys = np.vstack([X.ravel(), Y.ravel()]).T

z = np.zeros(len(_ys))
for pi, mu, sigma in zip(pis2, mus2, sigmas2):
    z += pi*mvn(mu, sigma).pdf(_ys)
z = z.reshape((intervals, intervals))

plt.figure(figsize=(10, 8))
ax = plt.subplot(111)
plt.scatter(xs[:,0], xs[:,1], alpha=0.2)
plt.contour(X, Y, z, N=10)
plt.axis([-8, 6, -6,8])
ax.axes.set_aspect('equal')
plt.tight_layout()
```

```python
%%time
ll3, pis3, mus3, sigmas3 = em_gmm_eins(xs, pis, mus, sigmas)
```

    CPU times: user 18 ms, sys: 1.48 ms, total: 19.4 ms
    Wall time: 20 ms

```python
%timeit em_gmm_orig(xs, pis, mus, sigmas)
%timeit em_gmm_vect(xs, pis, mus, sigmas)
%timeit em_gmm_eins(xs, pis, mus, sigmas)
```

    9.26 s ± 378 ms per loop (mean ± std. dev. of 7 runs, 1 loop each)
    17.1 ms ± 6.38 ms per loop (mean ± std. dev. of 7 runs, 100 loops each)
    17.7 ms ± 1.11 ms per loop (mean ± std. dev. of 7 runs, 10 loops each)

```python
intervals = 101
ys = np.linspace(-8,8,intervals)
X, Y = np.meshgrid(ys, ys)
_ys = np.vstack([X.ravel(), Y.ravel()]).T

z = np.zeros(len(_ys))
for pi, mu, sigma in zip(pis3, mus3, sigmas3):
    z += pi*mvn(mu, sigma).pdf(_ys)
z = z.reshape((intervals, intervals))

plt.figure(figsize=(10,8))
ax = plt.subplot(111)
plt.scatter(xs[:,0], xs[:,1], alpha=0.2)
plt.contour(X, Y, z, N=10)
plt.axis([-8,6,-6,8])
ax.axes.set_aspect('equal')
plt.tight_layout()
```

```python

```


```python
import sys
import sympy
from sympy import *
from sympy import Rational as frac
from sympy.tensor.tensor import TensorIndexType, tensor_indices, tensorhead
init_printing()

# Various scalars
vartheta, varphi = sympy.symbols('vartheta, varphi', real=True)
v, M, nu, delta, R_L = sympy.symbols('v, M, nu, delta, R_L', real=True)
Sigma_n, Sigma_ell, Sigma_lambda = sympy.symbols('Sigma_n, Sigma_ell, Sigma_lambda', real=True)
S_n, S_ell, S_lambda = sympy.symbols('S_n, S_ell, S_lambda', real=True)

# Tell sympy we're just doing Euclidean stuff
Euclidean = TensorIndexType('Euclidean', dummy_fmt='E')
Euclidean.data = [1,1,1]
g = Euclidean.metric
i0,i1,i2,i3 = tensor_indices('i0:4', Euclidean)

# Basis vectors of the source system
nHat, lambdaHat, ellHat = tensorhead(r'\hat{n}, \hat{\lambda}, \hat{\ell}', [Euclidean], [[1]])
nHat.data = [1,0,0]
lambdaHat.data = [0,1,0]
ellHat.data = [0,0,1]

# Spin vectors of the source system
Sigma,S = tensorhead(r'\vec{\Sigma}, \vec{S}', [Euclidean], [[1]])
Sigma.data = [Sigma_n, Sigma_lambda, Sigma_ell]
S.data = [S_n, S_lambda, S_ell]

# Basis vectors of the detector
NHat, PHat, QHat = tensorhead(r'\hat{N}, \hat{P}, \hat{Q}', [Euclidean], [[1]])
NHat.data = [sin(vartheta)*cos(varphi), sin(vartheta)*sin(varphi), cos(vartheta)]
PHat.data = [sin(varphi), -cos(varphi), 0]
QHat.data = [cos(vartheta)*cos(varphi), cos(vartheta)*sin(varphi), -sin(vartheta)]

# This projects the strain field onto the detector
D_jk = ( (PHat(i0)*PHat(i1) - QHat(i0)*QHat(i1)) - sympy.I*(PHat(i0)*QHat(i1) + QHat(i0)*PHat(i1)) )/2;
def complex_h_from_tensor(h_jk):
    return sympy.simplify((h_jk(i0,i1) * D_jk(i2,i3) * g(-i0,-i2) * g(-i1,-i3)).contract_metric(g))
    #return sympy.simplify(h_jk(i0,i1) * D_jk(-i0,-i1))

```

```python
h_jk = 4*(M/R_L)*nu*v**2*(lambdaHat(i0)*lambdaHat(i1) - nHat(i0)*nHat(i1))
```

```python
h_jk(i0,i1).data
```

    array([[-4*M*nu*v**2/R_L, 0, 0],
           [0, 4*M*nu*v**2/R_L, 0],
           [0, 0, 0]], dtype=object)

```python
h = complex_h_from_tensor(h_jk)
```


# Bayesian decision theory

```python
import numpy as np
np.set_printoptions(precision=3)

import pandas as pd
```

# From beliefs to actions

* Probabilistic inference let us compute our distribution over the possible **states of nature** $p(H=h|X=x)$; this is called our **belief state**. But ultimately we must convert beliefs into actions.
* The optimal way to make **decisions under uncertainty** is to use **decision theory**.
* If the **decision maker** or **agent** picks action $a \in \cal{A}$ when the true state of nature is $h \in \cal{H}$, then we assume they incur a **loss** of $\ell(h,a)$.
* Since the state is usually hidden, we must compute the  the **expected loss** or **risk** for each action:
$$
R(a|x) = \sum_{h \in \cal{H}} p(h|x) \ell(h,a)
$$
* The **optimal policy** specifies which action to take for each possible observation so as to minimize this risk:
$$
a^* = \pi^*(x)  = \arg \min_{a \in \cal{A}}
 \sum_{h \in \cal{H}} p(h|x) \ell(h,a)
$$
* Equivalently, we can **maximize expected utility**
$$
a^* = \pi^*(x) = \arg \max_{a \in \cal{A}} \sum_{h \in \cal{H}} p(h|x) U(h,a)
$$

# Example:  Optimal policy for treating COVID patients

* Suppose we are a doctor who can either do nothing or give an expensive and painful drug to a patient.
* Suppose the patient either has or does not have COVID, and is either young or old.
* Suppose the loss function $\ell(h,a)$ is as shown below, where the units are [Quality Adjusted Life Years](https://en.wikipedia.org/wiki/Quality-adjusted_life_year#:~:text=The%20quality%2Dadjusted%20life%20year,one%20year%20in%20perfect%20health.). This matrix encodes the assumption that a young person is likely to live much longer than an old person, and that the drug is so bad that it is like losing 8 (quality-adjusted) years of life. (We will vary these assumptions later.)

* We assume the age is a visible variable, but the COVID status is hidden.
* The doctor can use Bayes rule to infer $p(h|x)$, where $x$ is a diagnostic test.
* Given the belief state, we can compute the optimal policy, which is shown below.
* We see that we should only give the drug to young people who test positive.

## Sensitivity to assumptions

* If we reduce the cost of the drug from 8 to 5, we get the new optimal policy shown below.
* Now we see that we should give the drug to anyone who tests positive.

* If we keep the cost of the drug at 8, but increase the sensititivity of the test from 0.875 to 0.975, we get the new optimal policy shown below.
* Now we see that we should give the drug to anyone who tests positive, since the chance that they actually have COVID is higher.

## Code to reproduce the above.

```python
def make_loss_fun(loss_drug=15):
  loss_covid_young = 60
  loss_covid_old = 10
  loss_mat = np.zeros((4, 2))
  loss_mat[0, :] = [0, loss_drug]
  loss_mat[1, :] = [loss_covid_young, loss_drug]
  loss_mat[2, :] = [0, loss_drug]
  loss_mat[3, :] = [loss_covid_old, loss_drug]
  return loss_mat
```

```python
loss_mat = make_loss_fun()
records = []
records.append(('No COVID, young', loss_mat[0,0], loss_mat[0,1]))
records.append(('COVID, young', loss_mat[1,0], loss_mat[1,1]))
records.append(('No COVID, old', loss_mat[2,0], loss_mat[2,1]))
records.append(('COVID, old', loss_mat[3,0], loss_mat[3,1]))

df = pd.DataFrame.from_records(records, columns=['State', 'Nothing', 'Drugs'])
display(df)
```

      State
      Nothing
      Drugs

      0
      No COVID, young
      0.0
      15.0

      1
      COVID, young
      60.0
      15.0

      2
      No COVID, old
      0.0
      15.0

      3
      COVID, old
      10.0
      15.0

```python
def normalize(x):
  return x / np.sum(x)

def posterior_covid(observed, prevalence=None, sensitivity=None):
  # observed = 0 for negative test, 1 for positive test
  # hidden state = 0 if no-covid, 1 if have-covid
  if sensitivity is None:
    sensitivity = 0.875
  specificity = 0.975
  TPR = sensitivity;
  FNR = 1-TPR
  TNR = specificity
  FPR = 1-TNR
  # likelihood(hidden, obs)
  likelihood_fn = np.array([[TNR, FPR], [FNR, TPR]])
  # prior(hidden)
  if prevalence is None:
    prevalence = 0.1
  prior = np.array([1-prevalence, prevalence])
  likelihood = likelihood_fn[:, observed].T
  posterior = normalize(prior * likelihood)
  return posterior
```

```python
def compute_belief(obs, prevalence=None, sensitivity=None):
  test_result, age = obs
  # returns bel[0:3] for states (not-covid+not-old, covid+not-old, not-covid+old, covid+old)
  post = posterior_covid(test_result, prevalence, sensitivity)
  bel = np.zeros(4)
  if age==0:
    bel[0:2] = post
  else:
    bel[2:4] = post
  return bel

def policy(obs):
  bel = compute_belief(obs)
  loss_mat = make_loss_fun(loss_drug)
  expected_loss = np.dot(bel, loss_mat)
  return np.argmin(expected_loss)

```

```python
def make_table(cost_drug, prevalence=None, sensitivity=None):
  records = []
  for test_result in [0,1]:
    for age in [0,1]:
      obs = (test_result, age)
      bel = compute_belief(obs, prevalence, sensitivity)
      loss_mat = make_loss_fun(cost_drug)
      expected_loss = np.dot(bel, loss_mat)
      act = np.argmin(expected_loss)
      if age==0:
        post_covid = bel[1]
      else:
        post_covid = bel[3]
      records.append((test_result, age, post_covid, expected_loss[0], expected_loss[1], act))

  df = pd.DataFrame.from_records(records, columns=['test', 'age', 'pr(covid)', 'cost-noop', 'cost-drugs', 'action'])
  return df

```

```python
df = make_table(cost_drug=8)
display(df)
print(df.to_latex(index=False, float_format="%.2f" ))

```

      test
      age
      pr(covid)
      cost-noop
      cost-drugs
      action

      0
      0
      0
      0.014045
      0.842697
      8.0
      0

      1
      0
      1
      0.014045
      0.140449
      8.0
      0

      2
      1
      0
      0.795455
      47.727273
      8.0
      1

      3
      1
      1
      0.795455
      7.954545
      8.0
      0

    \begin{tabular}{rrrrrr}
    \toprule
     test &  age &  pr(covid) &  cost-noop &  cost-drugs &  action \\
    \midrule
        0 &    0 &       0.01 &       0.84 &        8.00 &       0 \\
        0 &    1 &       0.01 &       0.14 &        8.00 &       0 \\
        1 &    0 &       0.80 &      47.73 &        8.00 &       1 \\
        1 &    1 &       0.80 &       7.95 &        8.00 &       0 \\
    \bottomrule
    \end{tabular}

```python
df = make_table(cost_drug=5)
display(df)
print(df.to_latex(index=False, float_format="%.2f" ))
```

      test
      age
      pr(covid)
      cost-noop
      cost-drugs
      action

      0
      0
      0
      0.014045
      0.842697
      5.0
      0

      1
      0
      1
      0.014045
      0.140449
      5.0
      0

      2
      1
      0
      0.795455
      47.727273
      5.0
      1

      3
      1
      1
      0.795455
      7.954545
      5.0
      1

    \begin{tabular}{rrrrrr}
    \toprule
     test &  age &  pr(covid) &  cost-noop &  cost-drugs &  action \\
    \midrule
        0 &    0 &       0.01 &       0.84 &        5.00 &       0 \\
        0 &    1 &       0.01 &       0.14 &        5.00 &       0 \\
        1 &    0 &       0.80 &      47.73 &        5.00 &       1 \\
        1 &    1 &       0.80 &       7.95 &        5.00 &       1 \\
    \bottomrule
    \end{tabular}

```python
df = make_table(cost_drug=8, sensitivity=0.975)
display(df)
print(df.to_latex(index=False, float_format="%.2f" ))
```

      test
      age
      pr(covid)
      cost-noop
      cost-drugs
      action

      0
      0
      0
      0.002841
      0.170455
      8.0
      0

      1
      0
      1
      0.002841
      0.028409
      8.0
      0

      2
      1
      0
      0.812500
      48.750000
      8.0
      1

      3
      1
      1
      0.812500
      8.125000
      8.0
      1

    \begin{tabular}{rrrrrr}
    \toprule
     test &  age &  pr(covid) &  cost-noop &  cost-drugs &  action \\
    \midrule
        0 &    0 &       0.00 &       0.17 &        8.00 &       0 \\
        0 &    1 &       0.00 &       0.03 &        8.00 &       0 \\
        1 &    0 &       0.81 &      48.75 &        8.00 &       1 \\
        1 &    1 &       0.81 &       8.12 &        8.00 &       1 \\
    \bottomrule
    \end{tabular}

# Decision-theoretic classification

* In classification problems, the unknown state of nature is the "true" class label $y^* \in \cal{Y}$, where $\cal{Y} = \{1,\ldots,C\}$.
* We typically assume that the set of possible actions is to pick one of the labels, $\hat{y} \in \cal{Y}$.
* We often assume a **zero-one** loss matrix $\ell(h,a)$, as follows:

$$
\begin{array}{c|cc}
 & \hat{y}=0 & \hat{y}=1  \\ \hline
 y^*=0 & 0 & 1  \\
 y^*=1 & 1 & 0
 \end{array}
$$
* We can write this more concisely as follows:
$$
\ell(y^*,\hat{y}) = I(y^* \neq \hat{y})
$$
* In this case,
the posterior expected loss is
$$
R(\hat{y}|x)
 = p(\hat{y} \neq y^* |x) = 1-p(y^*=\hat{y}|x)
$$
Hence the action that minimizes the expected loss is to choose the
most probable label:
$$
\pi(x)  = \arg \max_{y \in \cal{Y}} p(y|x)
$$
* This  corresponds to
the **mode** of the posterior distribution,
also known as the **maximum a posteriori**
or  **MAP estimate**.

## Classification with a "reject" option

* In some applications (e.g., medicine, finance), we have an asymmetric loss, where the cost of a false positives is much higher than a cost of a false negative.
* In such settings, it is useful to allow for addition **rejection action**, in which we **refuse to classify** if we are uncertain. Such inputs can then be handled by some other system (e.g., a human).
* We define the loss function as follows:
$$
\ell(h, a) =
\left\{ \begin{array}{cc}
  0 & \mbox{if $h=a$ and $a \in \{1,\ldots,C\}$}\\
  \lambda_r & \mbox{if $a=0$}\\
  \lambda_e & \mbox{otherwise}
\end{array}
\right.
$$
where $\lambda_r$ is the cost of a rejection, and $\lambda_e$ is a cost of a misclassification error.
* It is easy to show that the optimal policy is as follows:
$$
\begin{align}
a^* &=
\begin{cases}
y^* & \text{ if } p(y^*|x) >  1-\frac{\lambda_r}{\lambda_e} \\
\text{reject} & \text{otherwise}
\end{cases}
\end{align}
$$
where $y^* = \arg \max_{y} p(y|x)$ is the MAP estimate.


# Tagging

## Tagging cells

### SIR model without vital dynamics

The [Susceptible-Infectious-Recovered (SIR) model](https://en.wikipedia.org/wiki/Compartmental_models_in_epidemiology) is an example of a compartmental model that is widely used to forecast the progress of disease epidemics.

\begin{align}
\frac{dS}{dt} & = -\frac{\beta I S}{N} \\
\frac{dI}{dt} & = \frac{\beta I S}{N} - \gamma I \\
\frac{dR}{dt} & = \gamma I
\end{align}

Where the **basic reproduction number** $R_0 = \frac{\beta}{\gamma}$

```python
%matplotlib inline

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp

N = 10000.0
R0 = 2.4
gamma = 1/8.0
beta = gamma*R0

def deriv(t, y):
    S, I, R = y
    dSdt = -beta*I*S/N
    dIdt = beta*I*S/N - gamma*I
    dRdt = gamma*I
    return [dSdt, dIdt, dRdt]

t = np.linspace(0, 180, 181)
y0 = [N - 1.0, 1.0, 0.0]
soln = solve_ivp(deriv, [t[0], t[-1]], y0, t_eval=t)

plt.plot(soln.t, soln.y[0], label='Susceptible')
plt.plot(soln.t, soln.y[1], label='Infectious')
plt.plot(soln.t, soln.y[2], label='Recoverd')
plt.legend()
plt.xlabel('days')
plt.grid(True)
```

# Special formatting for special tags

### SIR model without vital dynamics

The [Susceptible-Infectious-Recovered (SIR) model](https://en.wikipedia.org/wiki/Compartmental_models_in_epidemiology) is an example of a compartmental model that is widely used to forecast the progress of disease epidemics.

\begin{align}
\frac{dS}{dt} & = -\frac{\beta I S}{N} \\
\frac{dI}{dt} & = \frac{\beta I S}{N} - \gamma I \\
\frac{dR}{dt} & = \gamma I
\end{align}

Where the **basic reproduction number** $R_0 = \frac{\beta}{\gamma}$

This cell has been tagged with "home-activity".

This cell has been tagged with "class-activity".

This cell has been tagged with "important-note".

## Tagging solutions in code cells

For teaching purposes, it is often useful to selecting remove code from cells a

### Exercise 1.

In the following cell write a function that returns the square of a number.

```python
# write a function that returns the square of a number

### BEGIN SOLUTION
def sqr(x):
    return x**2
### END SOLUTION

assert sqr(2) == 4
assert sqr(-1) == 1

### BEGIN HIDDEN TESTS
assert sqr(3) == 9
assert abs(sqr(3.3) - 10.89) < 0.001
### END HIDDEN TESTS
```

```python
assert sqr('string') == 12
```


# 3D Visualization of calibrated cameras and a chessboard

This notebook visualizes the basics of perspective projection and pinhole cameras.

**Subjects covered**

1. **Camera calibration** using a chessboard and OpenCV's calibration functions.
2. **Interchanging coordinate frames**
    That is, switching between:
    * *3D world coordinate frame* - a frame with standard basis vectors centered at the world origin.
    * *2D camera image plane* - a frame centered at a camera's projection center and rotated
         to align the $z$-axis with the camera's principal axis, and the $x$ and $y$ axis with the camera's image plane.
    * *2D camera pixel plane* - another camera-centric coordinate frame, but with appropriate scaling, skewing,
        and translating to align coordinates of projected points with the camera's pixel coordinates of those points.
3. **Visualizing camera poses and a chessboard in 3D**.
4. **Superimposing an object into a scene**.
5. **Back-projecting image points** to exhibit the ray along which corresponding 3D object points will lie.
6. **Triangulating 3D world points** by finding the (near-)intersection of two rays of back-projected SIFT correspondences.
7. **Conclusion**
7. **Sources**

**Why are we interested in these subjects?**
Real-world cameras can be set up to approximate a pinhole camera. Pinhole camera image formation is well modeled by the maths of perspective projection.
This 2D image$\leftrightarrow$3D world correspondence enables us to infer world structure from images, and images from world structure.
This gives rise to a range of useful applications, including:
* measuring objects in the scene (photogrammetry)
* collision avoidance for autonomous robots (depth perception)
* inserting objects into a scene (virtual reality)
* superimposing textures onto a scene (virtual reality)
* creating 3D models of real-world objects (metric reconstruction).

**Prerequisites**
A basic familiarity with linear algebra, Python, and the camera matrix is assumed. For the camera matrix, reading [this OpenCV page](https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html ) should suffice.

**Further reading**
For a comprehensive treatment of perspective projection, see the first four chapters of [Multiple view geometry in computer vision](https://www.robots.ox.ac.uk/~vgg/hzbook/) by Richard Hartley and Andrew Zisserman. For a less thorough but more merciful introduction, watch [the second lecture](https://www.youtube.com/watch?v=iL4WhR0hzns&list=PL05umP7R6ij35L2MHGzis8AEHz7mg381_&ab_channel=T%C3%BCbingenMachineLearning) of Andreas Geiger's computer vision course.

## Imports

```python
import matplotlib.pyplot as plt
import ipyvolume as ipv
import numpy as np
import cv2
```

## Calibrate cameras
[Camera calibration](https://www.mathworks.com/help/vision/ug/camera-calibration.html) refers to the process of determining the extrinsic and intrinsic parameters of a camera. Extrinsic parameters model the pose of a camera in 3D space. Viewed as a transformation, the extrinsic matrix maps 3D points to the camera's image plane. Intrinsic parameters model how these 2D points in the image plane relate to the 2D pixel coordinates in the camera's image. Without knowing the intrinsics, one cannot infer object size, angles, parallelism, and other useful scene properties. Now we will see how to retrieve the intrinsic and extrinsic parameters.

As the camera extrinsics and instrinsics encode a 2D pixel$\leftrightarrow$3D world relation, knowing 2D$\leftrightarrow$3D correspondences lets us compute the parameters. To get 2D$\leftrightarrow$3D correspondences, a calibration object with known geometry such as a chessboard can be used. The OpenCV library has a function [findChessboardCorners](https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#ga93efa9b0aa890de240ca32b11253dd4a) that supports a chessboard setup. This means we do not need to manually label pixels to find 2D pixel$\leftrightarrow$3D world correspondences. To then calculate the intrinsics from correspondences, OpenCV's [calibrateCamera function](https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d) is used, which implements [Zhang's camera calibration algorithm](https://ieeexplore.ieee.org/abstract/document/888718/). For a high-level summary of this algorithm, see the appendix at the end of this notebook.

```python
def calibrate_cameras() :
    """ Calibrates cameras from chessboard images.

    Returns:
        images (list[np.ndarray]): Images containing the chessboard.
        intrinsics (np.ndarray): An upper triangular 4x4 full-rank matrix containing camera intrinsics.
        distortions (np.ndarray): Radial distortion coefficients.
        rotation_vectors (list[np.ndarray]): Rodrigues rotation vectors.
        translation_vectors (list[np.ndarray]): Translation vectors.
        object_points: (np.ndarray): A (4, 54) point array, representing the [x,y,z,w]
            of 54 chessboard points (homogenous coordiantes).
    """
    images = list()

    # Read images
    for i in range(11):
        img = cv2.imread(f'./images/{i}.jpg')
        img = cv2.resize(img, None, fx=0.25, fy=0.25)
        images.append(img)

    # The default opencv chessboard has 6 rows, 9 columns
    shape = (6, 9)

    # List to store vectors of 3D world points for every checkerboard image
    object_points_all = []

    # List to store vectors of 2D projected points for every checkerboard image
    image_points_all = []

    # Flags for chessboard corner search. Taken from opencv docs.
    flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE

    # Criteria for termination of the iterative corner refinement. Taken from opencv docs.
    refinement_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # List for images in which chessboard is be found by search
    images_filtered = list()

    # Object points in a single image. Simply a row iterated list of z=0 3d points.
    # E.g. [[0. 0. 0.] [1. 0. 0.] ... [0, 1, 0], [1, 1, 0], ... ]
    object_points = np.zeros((1, shape[0] * shape[1], 3), np.float32)
    object_points[0, :, :2] = np.mgrid[0:shape[0], 0:shape[1]].T.reshape(-1, 2)

    # For each image, store the object points and image points of chessboard corners.
    for idx, image in enumerate(images):
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        succes, corners = cv2.findChessboardCorners(image=gray,
                                                    patternSize=shape,
                                                    flags=flags)
        if succes:
            images_filtered.append(image)
            corners = cv2.cornerSubPix(image=gray,
                                       corners=corners,
                                       winSize=(11,11),
                                       zeroZone=(-1,-1),
                                       criteria=refinement_criteria)
            object_points_all.append(object_points)
            image_points_all.append(corners)

    images = images_filtered

    # Calibrate the cameras by using the 3D <-> 2D point correspondences.
    ret, intrinsics, distortions, rotation_vectors, translation_vectors = cv2.calibrateCamera(object_points_all, image_points_all, gray.shape[::-1], None, None)

    # Make intrinsic matrix 4x4 full-rank to ease manipulation.
    intrinsics = np.hstack([intrinsics, np.zeros((3, 1))])
    intrinsics = np.vstack([intrinsics, [[0, 0, 0, 1]]])

    # Convert chessboard object points to homogeneous coordinates to ease later use.
    object_points = object_points[0].reshape((-1, 3)).T
    object_points = np.vstack([object_points, np.ones((1, object_points.shape[1]))])

    return images, intrinsics, distortions, rotation_vectors, translation_vectors, object_points

images, intrinsics, distortions, rotation_vectors, translation_vectors, object_points = calibrate_cameras()

print(f'\n\nNumber of calibration images: {len(images)}')
print(f'Number of calibration points: {object_points.shape[1]}')
print(f'\n\nSample of imags used')
plt.figure(figsize=(20,20))
_ = plt.imshow(cv2.hconcat([cv2.cvtColor(im, cv2.COLOR_RGB2BGR) for im in images[:5]]))
```

## Convert calibration output to extrinsic matrices
The calibration method returns regular translations and rotations encoded as [Rodrigues rotation](https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula) vectors. After converting these rotation vectors into rotation matrices we can form $3 \times 4$ camera extrinsic matrices $[\mathbf{R}| \mathbf{t}] = \mathbf{E}$. Matrix multiplying the extrinsic matrix with a homogenous 3D point returns the point projected to the camera's image plane (a camera-centeric coordinate frame where $z=1$).

$$ \text{point in camera image plane} = [\mathbf{R}| \mathbf{t}] \cdot \text{point in world ref.}$$

The camera projection center is the origin of this camera-centric coordinate frame. This is the point mapped to zero by the extrinsics, i.e. the extrinsics' null space. Keep in mind that $\mathbf{EC} = \mathbf{RC} + \mathbf{T}$, that is, applying the extrinsic matrix to a homogenous point is the same as applying the rotation and adding a translation separately.

$$ \begin{align}
 0 &= \mathbf{EC}\\
 0 &= \mathbf{RC} + \mathbf{T}\\
-\mathbf{RC} &= \mathbf{T}\\
-\mathbf{R^T} \cdot -\mathbf{RC} &= -\mathbf{R^T}\mathbf{T}\\
 \mathbf{C} &= -\mathbf{R^T}\mathbf{T}
 \end{align}$$

This formula will now be used to infer the camera centers.

```python
def extrinsics_from_calibration(rotation_vectors, translation_vectors):
    """ Calculates extrinsic matrices from calibration output.

    Args:
        rotation_vectors (list[np.ndarray]): Rodrigues rotation vectors.
        translation_vectors (list[np.ndarray]): Translation vectors.
    Returns:
        extrinsics (list[np.ndarray]): A list of camera extrinsic matrices.
            These matrices are 4x4 full-rank.
    """

    rotation_matrices = list()
    for rot in rotation_vectors:
        rotation_matrices.append(cv2.Rodrigues(rot)[0])

    extrinsics = list()
    for rot, trans in zip(rotation_matrices, translation_vectors):
        extrinsic = np.concatenate([rot, trans], axis=1)
        extrinsic = np.vstack([extrinsic, [[0,0,0,1]]])
        extrinsics.append(extrinsic)

    return extrinsics

def camera_centers_from_extrinsics(extrinsics):
    """ Calculates camera centers from extrinsic matrices.

    Args:
        extrinsics (list[np.ndarray]):  A list of camera extrinsic matrices.
    Returns:
        camera_centers (list[np.ndarray]): Homogenous coordinates of camera centers in
            3D world coordinate frame.
    """
    camera_centers = list()

    for extrinsic in extrinsics:
        rot = extrinsic[:3, :3]
        trans = extrinsic[:3, 3]
        center = -rot.T @ trans
        center = np.append(center, 1)
        camera_centers.append(center)

    return camera_centers

extrinsics = extrinsics_from_calibration(rotation_vectors, translation_vectors)
camera_centers = camera_centers_from_extrinsics(extrinsics)

print(f'Shape of an extrinsic matrix: {extrinsics[0].shape}')
print(f'Shape of the intrinsic matrix: {intrinsics.shape}')
print(f'Number of cameras in the scene: {len(camera_centers)}')
```

## Invertible perspective projection
Perspective projection of a pinhole camera is performed in two steps. First, the 3D point is projected to the camera's image plane using the extrinsic matrix. Then, the point on the image plane is scaled, skewed, and tranlsated to align with the camera's image pixel coordinates using the intrinsic matrix.

$$ \begin{align}
\text{2D pixel point} &= \text{Align with pixels} (\text{Proj to cam image plane}(\text{3D world point})) \\
\mathbf{x}&=\mathbf{KEX} \\
\begin{bmatrix} x\\ y\\ w \end{bmatrix} &= \begin{bmatrix} f_{x} & \alpha     & c_{x0} \\
                                                            0    & f_{y} & c_{y0} \\
                                                            0    & 0     & 1  \end{bmatrix}
                                           \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_{x} \\
                                                           r_{24} & r_{25} & r_{26} & t_{y} \\
                                                           r_{36} & r_{37} & r_{38} & t_{z} \end{bmatrix}
                                           \begin{bmatrix} X\\ Y\\ Z\\ W \end{bmatrix}
\end{align}
$$
Often we refer to product $\mathbf{KE}$ as the projection matrix $\mathbf{P}$.

As one may notice, the extrinsic matrix is not square in this description, while in the code it is. The reason is that it's useful to be able to invert the projection matrix and its components, i.e. $\mathbf{P^{-1}}, \mathbf{E^{-1}}, \mathbf{K^{-1}}$. This allows easy switching between coordinate frames using matrix multiplication. To achieve this, add the row $(0, 0, 0, 1)$ to make $\mathbf{E}$ square and full-rank. Similarly, make $\mathbf{K}$ full-rank $4\times4$.

$$\mathbf{\tilde{P}} = \begin{bmatrix} \mathbf{K} &  0 \\ 0^{T} & 1 \end{bmatrix}
              \begin{bmatrix} \mathbf{R} &  t \\ 0^{T} & 1 \end{bmatrix} = \mathbf{\tilde{K}\tilde{E}}$$

The 4x4 projection matrix $\mathbf{\tilde{P}}$ can now be used to map directly from 3D world coordinates $\mathbf{\bar{p}} = (x_w, y_w, z_w,1)$ to screen coordinates plus disparity, $\mathbf{x_s} = (x_s, y_s, 1, d)$

$$ \mathbf{x_s} \sim \mathbf{\tilde{P}\bar{p}_w} $$

where $\sim$ indicates equality up to scale. Note that after multiplication by $\mathbf{\tilde{P}}$, the vector must be divided by the third element of the vector to obtain this normalized form $\mathbf{x_s} = (x_s, y_s, 1, d)$.

Whenever we don't want the full expressivity of a $4\times4$ projection matrix we can always slice the intrinsics and extrincs back to their original size. E.g. when we do not want to specify a disparity.

Partial excerpt taken from Computer Vision Algorithms and Applications by Richard Szeliski pg.49, section 'Camera matrix'.

## Plot chessboard and cameras
Given the extrinsics and a priori defined chessboard object points, a 3D visualization can be created. The [Ipyvolume](https://github.com/maartenbreddels/ipyvolume) library is used for this.

```python
cam_sphere_size = 1

def init_3d_plot():
    """ Initializes a ipyvolume 3d plot and centers the
        world view around the center of the chessboard.

    Returns:
        fig (ipyvolume.pylab.figure): A 3D plot.
    """
    chessboard_x_center = 2.5
    chessboard_y_center = 4
    fig = ipv.pylab.figure(figsize=(15, 15), width=800)
    ipv.xlim(2.5 - 30, 2.5 + 30)
    ipv.ylim(4 - 30, 4 + 30)
    ipv.zlim(-50, 10)
    ipv.pylab.view(azimuth=40, elevation=-150)
    return fig

def plot_chessboard(object_points):
    """ Plots a 3D chessboard and highlights the
        objects points with green spheres.

    Args:  A (4, 54) point array, representing the [x,y,z,w]
           of 54 chessboard points (homogenous coordiantes).
    """
    img = cv2.imread('./images/chessboard.jpg')
    img_height, img_width, _ = img.shape
    chessboard_rows, chessboard_cols = 7, 10
    xx, yy = np.meshgrid(np.linspace(0, chessboard_rows, img_height),
                         np.linspace(0, chessboard_cols, img_width))
    zz = np.zeros_like(yy)
    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) / 255
    # -1 is used as the start of the board images x and y coord,
    # such that the first inner corner appear as coord (0, 0, 0)
    ipv.plot_surface(xx-1, yy-1, zz, color=cv2.transpose(img))
    xs, ys, zs, _ = object_points
    ipv.scatter(xs, ys, zs, size=1, marker='sphere', color='lime')

def plot_camera_axis(cam_center, inv_extrinsic, vis_scale):
    """ Plots the x, y, and z basis vectors of the camera coordinate frame.
        This is achieved by  transforming the basis vectors into the world
        coordinate frame and plotting them.

    Args:
        cam_center (np.ndarray): Coordinates of camera centers in
            3D world coordinate frame.                                                                                :
        inv_extrinsic (np.ndarray): The (pseudo)-inverse of camera extrinsic matrix.
        vis_scale (int): A visualization size multiplyer to make cameras
            appear larger and easier to see.
    """
    x, y, z, _ = cam_center
    x_arrow = inv_extrinsic @ (1*vis_scale, 0, 0,  1)
    y_arrow = inv_extrinsic @ (0, 1*vis_scale, 0,  1)
    z_arrow = inv_extrinsic @ (0, 0, 1*vis_scale,  1)

    ipv.plot([x, x_arrow[0]], [y, x_arrow[1]], [z, x_arrow[2]], color='red')
    ipv.plot([x, y_arrow[0]], [y, y_arrow[1]], [z, y_arrow[2]], color='blue')
    ipv.plot([x, z_arrow[0]], [y, z_arrow[1]], [z, z_arrow[2]], color='green')
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), size=cam_sphere_size, marker="sphere", color='blue')
```

```python
init_3d_plot()
plot_chessboard(object_points)

# Determines the visual scale of the plotted cameras
vis_scale = 5

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    height, width, _ = image.shape
    inv_extrinsic = np.linalg.inv(extrinsic)
    plot_camera_axis(cam_center, inv_extrinsic, vis_scale)

ipv.show()
```

## Plot cameras as viewport wireframes
Instead of camera-centric coordinate frames, we can plot a slightly more intuitive wireframe of each camera's viewport.

```python
# Visual dimension of the camera in the 3D plot.
height, width, _ = images[0].shape
camera_aspect_ratio = width / height
# A length of 1 corresponds to the length of 1 chessboard cell.
# This is because a chessboard points have been defined as such.
# Set height of camera viewport to 1.
vis_cam_height = 1
vis_cam_width = vis_cam_height * camera_aspect_ratio
wire_frame_depth = 1.2

def plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic):
    """ Plots the wireframe of a camera's viewport. """
    x, y, z, _ = cam_center

    # Get left/right top/bottom wireframe coordinates
    # Use the inverse of the camera's extrinsic matrix to convert
    # coordinates relative to the camera to world coordinates.
    lt = inv_extrinsic @ np.array((-vis_cam_width/2, -vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    rt = inv_extrinsic @ np.array((vis_cam_width/2,  -vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    lb = inv_extrinsic @ np.array((-vis_cam_width/2,  vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale
    rb = inv_extrinsic @ np.array((vis_cam_width/2,   vis_cam_height/2, wire_frame_depth, 1/vis_scale)) * vis_scale

    # Connect camera projective center to wireframe extremities
    ipv.plot([x, lt[0]], [y, lt[1]], [z, lt[2]], color='blue')
    ipv.plot([x, rt[0]], [y, rt[1]], [z, rt[2]], color='blue')
    ipv.plot([x, lb[0]], [y, lb[1]], [z, lb[2]], color='blue')
    ipv.plot([x, rb[0]], [y, rb[1]], [z, rb[2]], color='blue')

    # Connect wireframe corners with a rectangle
    ipv.plot([lt[0], rt[0]], [lt[1], rt[1]], [lt[2], rt[2]], color='blue')
    ipv.plot([rt[0], rb[0]], [rt[1], rb[1]], [rt[2], rb[2]], color='blue')
    ipv.plot([rb[0], lb[0]], [rb[1], lb[1]], [rb[2], lb[2]], color='blue')
    ipv.plot([lb[0], lt[0]], [lb[1], lt[1]], [lb[2], lt[2]], color='blue')
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), size=cam_sphere_size, marker="sphere", color='blue')
```

```python
init_3d_plot()
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)

ipv.show()
```

## Plot images in the camera wireframes
To enhance the viewport visualization, let us
* add the real-world images to the viewport
* project the chessboard calibration points to the images

```python
def plot_picture(image, inv_extrinsic, vis_scale):
    """ Plots a real-world image its respective 3D camera wireframe. """
    image = image.copy()
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    image = cv2.resize(image, None, fx=0.1, fy=0.1) / 255
    img_height, img_width, _ = image.shape

    xx, yy = np.meshgrid(np.linspace(-vis_cam_width/2 * vis_scale,  vis_cam_width/2 * vis_scale,  img_width),
                         np.linspace(-vis_cam_height/2 * vis_scale, vis_cam_height/2 * vis_scale, img_height))
    zz = np.ones_like(yy) * wire_frame_depth * vis_scale
    coords = np.stack([xx, yy, zz, np.ones_like(zz)])
    coords = coords.reshape(4, -1)

    # Convert canera relative coordinates to world relative coordinates
    coords = inv_extrinsic @ coords
    xx, yy, zz, ones = coords.reshape(4, img_height, img_width)
    ipv.plot_surface(xx, yy, zz, color=image)

def project_points_to_picture(image, object_points, intrinsics, extrinsic):
    """ Perspective projects points to an image and draws them green. """
    image = image.copy()
    proj_matrix = intrinsics @ extrinsic
    object_points = proj_matrix @ object_points
    xs, ys, ones, disparity = object_points / object_points[2]

    for idx, (x, y) in enumerate(zip(xs, ys)):
        x = round(x)
        y = round(y)
        if (0 < y < image.shape[0] and
            0 < x < image.shape[1]):
            # Each point occupies a 20x20 pixel area in the image.
            image[y-10:y+10, x-10:x+10] = [0, 255, 0]

    return image
```

```python
init_3d_plot()
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image = project_points_to_picture(image, object_points, intrinsics, extrinsic)
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)

ipv.show()
```

## Place bunny model in scene and project to cameras

Now we can insert artificial 3D objects into the scene and project them to the cameras.
This is done for the infamous [Stanford bunny](https://www.cc.gatech.edu/~turk/bunny/bunny.html).
The bunny appears flat in the images because shading is not implemented and is out of this notebook's scope.

```python
def plot_bunny():
    """Plots the Stanford bunny pointcloud and returns its points.

    Returns:
        bunny_coords (np.ndarray): a 4xn homogenous point cloud array.
    """
    bunny_coords = np.load(open('data/bunny_point_cloud.npy', 'rb'))
    b_xs, b_ys, b_zs = bunny_coords[:3]
    bunny_point_size = 0.5
    ipv.scatter(b_xs, b_ys, b_zs, size=bunny_point_size, marker="sphere", color='lime')
    return bunny_coords
```

```python
init_3d_plot()
bunny_coords = plot_bunny()
plot_chessboard(object_points)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image = project_points_to_picture(image, object_points, intrinsics, extrinsic)
    image = project_points_to_picture(image, bunny_coords, intrinsics, extrinsic)
    inv_extrinsic = np.linalg.pinv(extrinsic)
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)

ipv.show()
```

## Plot camera optical axes
By using the inverse of the projection matrix, $\mathbf{P^{-1}}$, one can back-project the camera ray for a given image pixel.
In the following code, we back-project the center pixel of each camera. This should roughly equate to the camera's 'principal' or 'optical' axis.
Keep the reversed order of the extrinsics and intrinsics in mind.
$$\mathbf{P} = \mathbf{KE}$$
$$\mathbf{P^{-1}} = \mathbf{E^{-1} K^{-1}}$$

```python
init_3d_plot()
plot_chessboard(object_points)
dir_vec_length = 50
inv_intrinsics = np.linalg.inv(intrinsics)

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    image_center = np.array([image.shape[1]/2, image.shape[0]/2, 1, 1]).astype(int)

    # Color the center of the image green.
    x, y, _, _ = image_center
    image = image.copy()
    image[y-50:y+50, x-50:x+50] = [0, 255, 0]

    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ image_center

    # Increase the length of the ray (only x,y,z not w).
    # Scaling the whole homogenous vector would not change it.
    dir_vec_in_cam_ref[:3] = dir_vec_in_cam_ref[:3] * dir_vec_length
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center
    ipv.plot([cam_x, dir_vec_in_world[0]], [cam_y, dir_vec_in_world[1]], [cam_z, dir_vec_in_world[2]], color='red')

ipv.show()
```

```python
cx = intrinsics[0,2]
cy = intrinsics[1,2]
print(f'Optical center according to the intrinsic matrix.\n x:{cx:.1f} y:{cy:.1f}\n')
print(f'Optical center according to the images.\n x:{x:.1f} y:{y:.1f} (assuming optical center is in middle)\n')
print(f'Discrepancy: \n x:{abs(cx-x):.1f} y:{abs(cy-y):.1f}')
```

## Plot all camera rays towards the first inner corner of the chessboard
Note that because we
* keep track of the points 'disparity' after projection
* use a full rank $4\times4$ invertible projection matrix

we can back-project a camera ray with a length that reaches the appropriate world point.

```python
init_3d_plot()
plot_chessboard(object_points)
chess_corner = np.array([0, 0, 0, 1])

for cam_center, extrinsic, image in zip(camera_centers, extrinsics, images):
    proj_matrix = intrinsics @ extrinsic
    chess_corner_proj = proj_matrix @ chess_corner
    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ chess_corner_proj
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center
    ipv.plot([cam_x, dir_vec_in_world[0]], [cam_y, dir_vec_in_world[1]], [cam_z, dir_vec_in_world[2]], color='red')

ipv.show()
```

## Triangulate 3D points on the table with image point correspondences

[Triangulation](https://en.wikipedia.org/wiki/Triangulation) in computer vision refers to the process of determining the location of a world point by finding near-intersections of camera rays. In this section we will:
* find [SIFT](https://en.wikipedia.org/wiki/Scale-invariant_feature_transform) point correspondences in a pair of images.
* back-project the image points into rays.
* find the triangulated point where the rays intersect.

## Handpick correct SIFT correspondences
Normally, one would use an automatic and robust method to find correspondences across multiple images.
Due to the
* large difference in camera poses
* poor exposure of the images
* absence of texture features on the table

valid point correspondences will be hand-picked in this code cell.

```python
camera_1_idx = 3
camera_2_idx = 0
image_1 = images[camera_1_idx].copy()
image_2 = images[camera_2_idx].copy()

# Initialize SIFT detector
sift = cv2.SIFT_create()

# Find the keypoints and descriptors with SIFT
keypoints_1, descriptors_1 = sift.detectAndCompute(image_1, None) # queryImage
keypoints_2, descriptors_2 = sift.detectAndCompute(image_2, None) # trainimage

# Match descriptors
bf = cv2.BFMatcher()
matches = bf.knnMatch(descriptors_1, descriptors_2, k=2)

# Apply ratio test
good = []
for best_match, second_best_match in matches:
    if best_match.distance < 0.75 * second_best_match.distance:
        good.append([best_match])

# Sort matches according to descriptor distance
dists = [g[0].distance for g in good]
good = list(sorted(zip(dists, good)))
good = [list(g) for g in zip(*good)][1]

# Select manually validated matches
hand_picked_matches = [2, 9, 15, 16, 18, 19, 22, 23, 24]
good = np.array(good, dtype=object)[hand_picked_matches]

# Plot matches
match_image = cv2.drawMatchesKnn(image_1, keypoints_1, image_2, keypoints_2, good, None, [0, 255, 0])
match_image = cv2.cvtColor(match_image, cv2.COLOR_RGB2BGR)
plt.figure(figsize=(20,20))
plt.imshow(match_image)
plt.show()
```

## Encode and plot matches as image points

```python
match_coords_1 = list()
match_coords_2 = list()

for i in good:
    i = i[0]
    keypoint_1 = keypoints_1[i.queryIdx]
    keypoint_2 = keypoints_2[i.trainIdx]
    keypoint_1_center = np.array(keypoint_1.pt)
    keypoint_2_center = np.array(keypoint_2.pt)
    x1, y1 = keypoint_1_center
    x2, y2 = keypoint_2_center
    match_coords_1.append([x1, y1, 1, 1])
    match_coords_2.append([x2, y2, 1, 1])
    color = [0, 0, 255]
    image1 = cv2.circle(image_1, keypoint_1_center.astype(int), 10, color, -1)
    image2 = cv2.circle(image_2, keypoint_2_center.astype(int), 10, color, -1)

match_coords_1 = np.array(match_coords_1)
match_coords_2 = np.array(match_coords_2)

image_1 = cv2.cvtColor(image_1, cv2.COLOR_BGR2RGB)
image_2 = cv2.cvtColor(image_2, cv2.COLOR_BGR2RGB)

image_concat = cv2.hconcat([image_1, image_2])
plt.figure(figsize=(20,20))
plt.imshow(image_concat)
plt.show()
```

## Draw back-projected rays of point correspondences
Rays from the first camera are drawn in red, those from the second camera are drawn in green.

```python
camera_1_idx = 3
camera_2_idx = 0

init_3d_plot()
plot_chessboard(object_points)

for idx, (cam_center, extrinsic, image) in enumerate(zip(camera_centers, extrinsics, images)):

    if idx == camera_1_idx:
        points = match_coords_1
        color = 'red'
    elif idx == camera_2_idx:
        points = match_coords_2
        color = 'lime'
    else:
        continue

    inv_extrinsic = np.linalg.inv(extrinsic)
    dir_vec_in_cam_ref = inv_intrinsics @ points.T
    dir_vec_in_cam_ref[:3] *= 55
    dir_vec_in_world = inv_extrinsic @ dir_vec_in_cam_ref
    plot_camera_wireframe(cam_center, vis_scale, inv_extrinsic)
    plot_picture(image, inv_extrinsic, vis_scale)
    cam_x, cam_y, cam_z, _ = cam_center

    for vec in dir_vec_in_world.T:
        ipv.plot([cam_x, vec[0]], [cam_y, vec[1]], [cam_z, vec[2]], color=color)

ipv.show()
```

# Triangulate, i.e. find the shortest line between two lines in 3D
Given the
* stereo point correspondences
* intrinsic matrix of both cameras
* extrinsic matrix of both cameras

pixel points can be back-projected into rays. The intersections of these corresponding rays are the triangulated 3D point.
The mathematics of finding this shortest line between two lines in 3D is detailed (with code examples) at http://paulbourke.net/geometry/pointlineplane/.
We will take the midpoint of the shortest line joining two rays as the triangulated point, denoted here as $\mathbf{m}$.

Here $P_{a}$ and $P_{b}$ are the desired closest points on lines $a$ and $b$ respectively.
$$ P_{a} = P_{1} + \mu_{a} (P2 - P1)$$
$$ P_{b} = P_{3} + \mu_{b} (P4 - P3)$$

In our case, $P_{1}$ and $P_{3}$ are the first and second camera centers.
$P_{2}$ and $P_{4}$ are the back-projected correspondence pixels in the first and second camera.

Solving for $\mu_{a}$ and $\mu_{b}$ provides the desired points on the lines. $\mu_{a}$ and $\mu_{b}$ have the following closed-form solution.
Let:
$$P_{12} \stackrel{\text{def}}{=} P_{1} - P_{2} $$
$$d_{1234} \stackrel{\text{def}}{=} P_{12} \cdot P_{34} $$

Then:
$$ \mu_{a} = \frac{d_{1343} d_{4321} - d_{1321} d_{4343}}{d_{2121} d_{4343} - d_{4321} d_{4321}}$$

$$ \mu_{b} = \frac{ d_{1343} + \mu_{a} d_{4321}}  {d_{4343}}   $$

See [this link](http://paulbourke.net/geometry/pointlineplane/) for the derivation. Note that the derivation is not complete, as it does not show the expansion of terms. This can be easily shown, however, by finding the reduced row echelon form of the equations that are derived.

```python
def triangulate(p1, p2, p3, p4):
    """ Calculates the point triangulated by two lines.
        Also returns that points projection onto line 1 and 2,
        know as Pa and Pb in the above description.
    """
    # Strip potential scale factor of homogenous coord
    p1 = p1[:3]
    p2 = p2[:3]
    p3 = p3[:3]
    p4 = p4[:3]

    p13 = p1 - p3
    p21 = p2 - p1
    p43 = p4 - p3

    d1321 = np.dot(p13, p21)
    d1343 = np.dot(p13, p43)
    d2121 = np.dot(p21, p21)
    d4321 = np.dot(p43, p21)
    d4343 = np.dot(p43, p43)

    mu_a = (d1343 * d4321 - d1321 * d4343) / (d2121 * d4343 - d4321 * d4321)
    mu_b = (d1343 + mu_a * d4321) / d4343

    point_on_line_1 = p1 + mu_a * p21
    point_on_line_2 = p3 + mu_b * p43

    adjoining_line = point_on_line_2 - point_on_line_1
    midpoint = adjoining_line / 2

    triangulated_point = point_on_line_1 + midpoint

    return triangulated_point, point_on_line_1, point_on_line_2
```

## Plot triangulated points

```python
image_1 = images[camera_1_idx]
image_2 = images[camera_2_idx]
extrinsic_1 = extrinsics[camera_1_idx]
extrinsic_2 = extrinsics[camera_2_idx]
inv_extrinsic_1 = np.linalg.inv(extrinsic_1)
inv_extrinsic_2 = np.linalg.inv(extrinsic_2)
cam_center_1 = camera_centers[camera_1_idx]
cam_center_2 = camera_centers[camera_2_idx]

for coord_1, coord_2 in zip(match_coords_1, match_coords_2):
    dir_vec_in_cam_ref_1 = inv_intrinsics @ coord_1
    dir_vec_in_cam_ref_2 = inv_intrinsics @ coord_2
    vec_1 = inv_extrinsic_1 @ dir_vec_in_cam_ref_1
    vec_2 = inv_extrinsic_2 @ dir_vec_in_cam_ref_2
    triangulated, _, _ = triangulate(cam_center_1, vec_1, cam_center_2, vec_2)
    x, y, z = triangulated
    print(f'x: {x:.1f},  \ty: {y:.1f},  \tz: {z:.1f}')
    ipv.scatter(np.array([x]), np.array([y]), np.array([z]), marker="sphere")

ipv.show()
```

####

## Conclusion
The triangulation was a success!
The rays align as expected, with the height of points on the table being consistently slightly under the chessboard.
This presents many possibilities. By knowing the real-world length of the chessboard, given enough point correspondences, we can:
* infer the size and shape of the table, candles, and other objects in the scene.
* insert objects into the scene and make them appear geometrically consistent in the cameras (as done with the bunny).
* project patterns onto objects in a geometrically consistent manner (like dynamic selfie filters).

While this method of triangulation is convenient for our current setup, it assumes cameras poses and point correspondences are given and correct.
A more convenient method for determining 3D world points would allow:
* iteratively refining camera poses and 3D world points simultaneously.
* utilizing the point correspondences in more than 2 images for preciser and more robust triangulation.

Bundle adjustment provides these benefits and will be demonstrated in notebook 4.

This concludes the first notebook.
Having a visualization throughout each step in this notebook has made for a pleasant journey. Rather than taking the matrix manipulations and geometry theory at face value, we get to see and interact with it. Also, this can save quite some time when debugging. This is feasible thanks to [Ipyvolume](https://github.com/maartenbreddels/ipyvolume), which provides efficient OpenGL interfacing (unfortunately, Matplotlib doesn't support z-buffering in 3D mode).

Next up: a notebook on unit quaternions as 3D rotation representations.

## Sources

1. [Szeliski, Richard. "Computer vision: algorithms and applications." Springer Science & Business Media, 2010](https://szeliski.org/Book/)
2. [Zisserman, Richard Hartley Andrew. "Multiple view geometry in computer vision." 2004.](https://www.robots.ox.ac.uk/~vgg/hzbook/)
3. [Zhang, Zhengyou. "A flexible new technique for camera calibration." IEEE, 2000](https://ieeexplore.ieee.org/abstract/document/888718)
4. [OpenCV Documentaiton](https://docs.opencv.org/4.5.4/)
5. [Paul Bourke Geometry Website](http://paulbourke.net/geometry/pointlineplane/)
6. [Ipyvolume by Maarten Breddels](https://github.com/maartenbreddels/ipyvolume)
7. [Wikipedia](https://www.wikipedia.org/). Article links included in text.

## Appendix

### Zhang's camera calibration algorithm
Zhang's algorithm models camera intrinsics and extrinsics by:
* Imposing constraints on the camera matrices by associating known world points on a calibration object with imaged pixel coordinates.
* Imposing the constraint of a shared intrinsic matrix across all cameras.
* Using an analytical solution to gain an initial estimate of each camera's pose and shared intrinsics. Distortions are not yet considered.
* Refining the initial estimate with an iterative non-linear optimization of the reprojection error of the calibration points. This step also does not consider distortions.
    The Levenberg-Marquardt optimization algorithm is used. This algorithm iteratively
    1. Computes the derivatives (Jacobian $J$) of the squared reprojection error w.r.t the camera parameters up to the second-order. Zhang used [Minpack](https://devernay.github.io/cminpack/), so numerical differentiation was used. For numerical differentiation, the second-order derivatives (Hessian $H$) are estimated using the [Jacobian product](https://math.stackexchange.com/questions/2349026/why-is-the-approximation-of-hessian-jtj-reasonable) $H = J^{T}J$.
    2. Set the derivative of the squared error function to be zero. This is the analytical least-squares solution. That is, the reprojection error function is being modeled as a locally quadratic, ideally convex, function. This loss function will have the lowest loss where the derivative is zero, that is, where the loss lies in a valley, or a saddlepoint in the non-ideal case. This solution tends to miss or overshoot the optimal parameters when the initial guess of camera parameters is poor. This is due to the non-quadratic and even non-euclidean nature of the reprojection error w.r.t camera parameters. To solve this see step 3.
    3. Determine a dampening factor $\lambda$. This factor will determine the tradeoff between the analytical least-squares solution and the more stable but less optimal gradient-descent update for camera parameters. When $\lambda=0$ only the analytical least-squares solution is used. This is known as [Guass-Newton optimization](https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm). For a greater $\lambda$, the update is closer to a gradient-descent step. This is known as [Levenberg-Marquardt optimization](https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm).
    4. Update the camera parameters. Observe the new points projections and reprojection errors. Iterate until convergence.
* Finally, estimate radial distortion by another round of Levenberg-Marquardt minimization of the reprojection error. That is, introduce distortion parameters into the image formation model and reoptimize.


# Vibration System Objects

[Download This Notebook](http://vibrationtoolbox.github.io/vibration_toolbox/_downloads/vibesystem_notebook.ipynb)

```python
import numpy as np
import vibration_toolbox as vtb
%matplotlib notebook
```

This notebook will introduce the class VibeSystem(), which is available in the vibration_toolbox.
As an example we will use the following 3 degree of freedom system:

If we look at the help for the VibeSystem class we have:
```
Parameters
----------
M : array
    Mass matrix.
C : array
    Damping matrix.
K : array
    Stiffness matrix.
name : str, optional
    Name of the system.
```
So we need to get the mass, stiffness and damping matrix to create a VibeSystem object (the name of the system is optional and can be used latter to help when we plot results).

For this system the kinetic energy is:

\begin{equation}
T = \frac{1}{2}[m_0\dot{q_0}(t)^2 + m_1\dot{q_1}(t)^2 + m_2\dot{q_2}(t)^2] = \frac{1}{2}{\bf \dot{q}}^T(t)\ M \ {\bf \dot{q}}(t)
\end{equation}

where ${\bf q(t)} = [q_0(t) \  q_1(t) \ q_2(t)]^T$ is the configuration vector. The mass matrix for this system is given by:

\begin{equation}
M =
\begin{bmatrix}
        m_0 & 0 & 0\\
        0 & m_1 & 0 \\
        0 & 0 & m_2
\end{bmatrix}
\end{equation}

The potential energy is:

\begin{equation}
V = \frac{1}{2}[k_0 q_0(t)^2 + k_1(q_1(t) - q_0(t))^2 + k_2 (q_2(t) - q_1(t))^2]
\end{equation}

\begin{equation}
= \frac{1}{2}[(k_0+k_1)q_0(t)^2 + (k_1+k_2)q_1(t)^2 + (k_2)q_2(t)^2 -2k_1 q_0(t)q_1(t) - 2k_2 q_2(t)] \\
\end{equation}

\begin{equation}
= \frac{1}{2}{\bf {q}}^T(t)\ K \ {\bf {q}}(t)
\end{equation}

And the stiffness matrix is:

\begin{equation}
K =
\begin{bmatrix}
        k_0 +k_1 & -k_1 & 0\\
        -k_1 & k_1+k_2 & -k_2 \\
        0 & -k_2 & k_2
\end{bmatrix}
\end{equation}

In this case we will consider proportional damping: $C = \alpha M + \beta K$.

Let's consider the following values for our system:

```python
m0, m1, m2 = (1, 1, 1)
k0, k1, k2 = (1600, 1600, 1600)
alpha, beta = 1e-3, 1e-3
```

Now we use numpy to create our matrices:

```python
M = np.array([[m0, 0, 0],
              [0, m1, 0],
              [0, 0, m2]])
K = np.array([[k0+k1, -k1,   0],
              [-k1, k1+k2, -k2],
              [0,     -k2,  k2]])
C = alpha*M + beta*K
```

```python
sys = vtb.VibeSystem(M, C, K, name='3 dof system')
```

Now we have everything needed to create a vibration system object:

Now, if we type **```sys.```** and press ```tab``` we can see the system's attributes and methods that are available.

As an example we can get the natural frequencies:

```python
sys.wn
```

    array([ 17.80167472,  49.87918415,  72.07750943])

Or the damped natural frequencies:

```python
sys.wd
```

    array([ 17.80096508,  49.86365725,  72.03066935])

We can also check the frequency response for specific input output pairs:

```python
ax = sys.plot_freq_response(0, 0)
```

    <IPython.core.display.Javascript object>

M, C and K can be changed and the system will be updated:

```python
sys.C = 20*C
sys.wd
```

    array([ 17.51552471,  43.22562039,  49.95149484])

```python
ax = sys.plot_freq_response(0, 0)
```

    <IPython.core.display.Javascript object>

```python
sys.C = C
axs = sys.plot_freq_response_grid([0, 1], [0, 1])
```

    <IPython.core.display.Javascript object>

For the plot functions, the axes that will be used can be provided. This can help us when we want to show two different results on the same plot.
This is ilustrated below by plotting the frequency response first using only the first two modes:
```python
ax = sys.plot_freq_response(0, 0, modes=[0, 1],
                            color='r', linestyle='--',
                            label='Modes 0 and 1')
```
Notice that the axes with magnitude and phase were returned and assigned to the 'ax' name.

Now we pass 'ax' to plot the frequency response with all the modes (default when modes are not provided).
```python
ax = sys.plot_freq_response(0, 0, ax0=ax[0], ax1=ax[1],
                            color='b', label='All modes')
```
We can check the results below:

```python
ax = sys.plot_freq_response(0, 0, modes=[0, 1],
                            color='r', linestyle='--',
                            label='Modes 0 and 1')
ax = sys.plot_freq_response(0, 0, ax0=ax[0], ax1=ax[1],
                            color='b', label='All modes')
ax[1].legend();
```

    <IPython.core.display.Javascript object>

The time response for the system can also be plotted. For that we need to define a time array and the force that will be applied during that time. The force should be given as an array where each row corresponds to a period of time and each column to where the force is being applied.

```python
t = np.linspace(0, 25, 1000)
# force array with len(t) rows and len(inputs) columns
F1 = np.zeros((len(t), 3))
# in this case we apply the force only to the mass m1
F1[:, 1] = 1000*np.sin(40*t)
```

```python
axs = sys.plot_time_response(F1, t)
```

    <IPython.core.display.Javascript object>

We can use the plot_fft function available in the toolbox to see the FFT from the time response.

```python
t, yout, xout = sys.time_response(F1, t)
ax = vtb.plot_fft(t, time_response=yout[:, 0])
```

    <IPython.core.display.Javascript object>

```python

```


## Zakharov

The Zakharov function has no local minima except the global one. It is shown here in its two-dimensional form.

**Definition**

\begin{align}
\begin{split}
f(x) &=& \sum\limits_{i=1}^n {x_i^2} + \bigg( \frac{1}{2} \sum\limits_{i=1}^n {ix_i} \bigg)^2 + \bigg( \frac{1}{2} \sum\limits_{i=1}^n {ix_i} \bigg)^4, \\[2mm]
&& -10 \leq x_i \leq 10 \quad i=1,\ldots,n
\end{split}
\end{align}

**Optimum**

$$f(x^*) = 0 \; \text{at} \; x^* = (0,\ldots,0) $$

**Contour**

```python
import numpy as np
from pymoo.factory import get_problem, get_visualization

problem = get_problem("zakharov", n_var=2)
get_visualization("fitness-landscape", problem, angle=(45, 45), _type="surface").show()
get_visualization("fitness-landscape", problem, _type="contour", contour_levels = 200, colorbar=True).show()
```


# Model Project

We start by importing necessary packages:

```python
import pandas as pd
import numpy as np

import matplotlib.pyplot as plt
import ipywidgets as widgets
import time
from scipy import linalg
from scipy import optimize
import sympy as sm
```

# Solow model with climate change

Consider the standard Solow-model with an indicator for climate change where:

$K_t$ is capital
    $L_t$ is labor (growing with a constant rate of $n$)
    $A_t$ is technology (growing with a constant rate of $g$)
    $D_t$ is the damage function (growing with a constant rate of $n$)
    $Y_t = F(K_t,A_t,L_t,D_t)$ is GDP

The Damage-function depends on temperature T

$$ D_t = 1-\frac{1}{1+\theta_1T_t^{\theta_2}} =< 1 $$

$T_t^{\theta_2}$ is the global mean temperature change at time t. $\theta_1$ is the damage parameter.

Cobb-Douglas production-function

$$ F(K_{t},A_{t}L_{t},D_{t})=  (1-D_{t})A_{t}K_{t}^{\alpha}L_{t}^{1-\alpha}$$

It is convenient to express all variables of interest in terms of per effective worker, as this can we used as a measure of welfare of sociely. We use small letters to denote per effective worker, so $y=Y/AL$ and $k=K/AL $, which gives:

$$ f({k}_{t})=(1-D){k}_{t}^{\alpha} $$

Saving is a constant fraction of GDP

$$ S_t = sY_t,\,s\in(0,1) $$

 **Capital accumulates** according to

\\[ K_{t+1}=S_{t}+(1-\delta)K_{t}=sF(K_{t},A_{t}L_{t})+(1-\delta)K_{t}, \delta \in (0,1) \\]

The economy is growing over time (due to exogenous technological progress and population growth), which makes it useful to focus on the behavior of capital stock per unit of effective labor. The transition in the model can be decribed by

\\[ \tilde{k}_{t+1}= \frac{1}{(1+n)(1+g)}[sf(\tilde{k}_{t})+(1-\delta)\tilde{k}_{t}] \\]

# Level of output for changes in K at different levels of Damage

The output level depends on the damage. Therefore, different levels of damage will lead to different levels of output.

Define parameters and arrays

```python
K_size = 100                # Model domain
alpha = (1/3)
D = 0.1

k = np.arange(K_size)

```

Output values

```python
def output(k, D):
    y = (1-D)  * (k)**(alpha)
    return y

y  = output(k, D)
y2 = output(k, D + 0.1)
y3 = output(k, D + 0.2)
y4 = output(k, D + 0.3)
y5 = output(k, D + 0.4)
y6 = output(k, D + 0.5)
```

Plot the production function for different values of damage

```python
fig, ax = plt.subplots(figsize=(12, 12))
ax.set(title="Output", xlabel="Capital", ylabel="Output")
ax.set_ylim([0,20])
ax.grid()
ax.plot(k, y,  "b-", alpha=1.00, label="D= 0.1")
ax.plot(k, y2, "b-", alpha=0.85, label="D= 0.2")
ax.plot(k, y3, "b-", alpha=0.70, label="D= 0.3")
ax.plot(k, y4, "b-", alpha=0.55, label="D= 0.4")
ax.plot(k, y5, "b-", alpha=0.40, label="D= 0.5")
ax.plot(k, y6, "b-", alpha=0.25, label="D= 0.6")
ax.legend()
ax.set_ylim(0, 6)
ax.set_xlim(0, 100)
plt.show()
```

With an increase in damage the output decreases. The level of damage seems to have an immense  effect on the output.

# Graph: The Steady-State (Model with Climate Change)

In the following cells we are going to code the graph for our solow model with climate change. The graph shows the output, investments (=savings) and depreciation in our model.

Define parameters and arrays

```python
K_size = 101
A = 1
L = 5
alpha = (1/3)
s = 0.2
delta = 0.10
g = 0.02
n = 0.01
D_ = 0.1

K = np.arange(K_size)            # Create empty array of K
```

Define Functions

```python
def output(K):   # Cobb-Douglas Production Function
    Y = (1-D_) * A * (K)**(alpha) * (L)**(1-alpha)
    return Y
```

Populate arrays

```python
Y = output(K)
D = (delta)*K
S = s*Y
```

Steady State Values

In a closed economy with no government, consumption per capita is the difference between income and investment: $$c=y−i$$

There is a capital depreciation: $$ d_t = {\delta}*k_t $$

```python
Kstar = ((delta+(g*n)+n+g)/s*(1-D_))**(1/(alpha-1))
Ystar = (1-D_)*Kstar**alpha
Sstar = s*Ystar
Cstar = Ystar - Sstar
Dstar = (delta)*Kstar
```

Plot the graph

```python

fig, ax = plt.subplots(figsize=(10, 8))
ax.plot(K, Y, "k", ls = '-', label="Output")
ax.plot(K, S, "b", ls = '-', label="Investment")
ax.plot(K, D, "r", ls = '-', label="Depreciation")
ax.set(title="Solow Model", xlabel="Capital Stock")
plt.text(77, 12,  r'$Y = A \cdot K^{\alpha} L^{1-\alpha}$')
plt.text(90, 10,  r'$D = {\delta}K$')
plt.text(90, 3, r'$S = sY$')
plt.legend(loc=2)
ax.set_xlim(0, 100)
ax.set_ylim(0, 15)
plt.show()
```

# Behavior of capital stock per unit of effective labor over time: Comparing Model with and without Climate Change

## Model including climate change

```python
k = sm.symbols('k')
alpha = sm.symbols('alpha')
delta = sm.symbols('delta')
s = sm.symbols('s')
A = sm.symbols('A')
D = sm.symbols('D')
n = sm.symbols('n')
g = sm.symbols('g')

sm.init_printing(use_unicode=True)
```

Equation of capital stock per unit of effective labour

```python
ss1 = sm.Eq(k,(s*((1-D)*((k)**alpha))+(1-delta)*k)/((1+n)*(1+g)))
ss1
```

Create a vector for time, t

```python
x_vec = np.zeros((100,1))
```

```python
x_vec[0] = 1
```

Then we select some parameter values, Let
$$s=0.2 $$
$$ g=0.02 $$
$$ n=0.01 $$
$$ delta=0.1$$
$$ alpha=1/3 $$
$$ D=0,1 $$

```python
s = 0.2
g = 0.02
n = 0.01
delta = 0.1
alpha = (1/3)
D = 0.1
```

Now, plug vector x_vec into the capital transition equation:

```python
for i in range(1, 100):
    x_vec[i] = (s*(1-D)*x_vec[i-1]**alpha+(1-delta)*x_vec[i-1])/((1+g)*(1+n))
```

Now we can check whether the x_vec [steady state capital] converges to a steady state.

```python
x_vec
```

    array([[1.        ],
           [1.04834013],
           [1.09334207],
           [1.13516079],
           [1.17396056],
           [1.20991008],
           [1.24317873],
           [1.27393383],
           [1.30233866],
           [1.328551  ],
           [1.35272218],
           [1.3749964 ],
           [1.39551038],
           [1.4143932 ],
           [1.43176626],
           [1.44774341],
           [1.46243112],
           [1.47592874],
           [1.48832876],
           [1.49971719],
           [1.51017383],
           [1.51977267],
           [1.5285822 ],
           [1.53666575],
           [1.54408184],
           [1.5508845 ],
           [1.55712355],
           [1.56284493],
           [1.56809096],
           [1.57290059],
           [1.5773097 ],
           [1.58135125],
           [1.58505559],
           [1.58845058],
           [1.59156185],
           [1.59441292],
           [1.5970254 ],
           [1.59941913],
           [1.60161232],
           [1.60362169],
           [1.60546255],
           [1.60714899],
           [1.60869391],
           [1.61010912],
           [1.6114055 ],
           [1.61259299],
           [1.6136807 ],
           [1.61467699],
           [1.61558955],
           [1.61642538],
           [1.61719092],
           [1.61789208],
           [1.61853426],
           [1.61912241],
           [1.61966108],
           [1.62015441],
           [1.62060623],
           [1.62102003],
           [1.62139899],
           [1.62174605],
           [1.62206389],
           [1.62235497],
           [1.62262154],
           [1.62286567],
           [1.62308924],
           [1.62329398],
           [1.62348148],
           [1.62365319],
           [1.62381044],
           [1.62395444],
           [1.62408632],
           [1.62420708],
           [1.62431768],
           [1.62441896],
           [1.6245117 ],
           [1.62459664],
           [1.62467442],
           [1.62474564],
           [1.62481087],
           [1.6248706 ],
           [1.6249253 ],
           [1.6249754 ],
           [1.62502127],
           [1.62506328],
           [1.62510174],
           [1.62513697],
           [1.62516923],
           [1.62519877],
           [1.62522583],
           [1.6252506 ],
           [1.62527329],
           [1.62529406],
           [1.62531309],
           [1.62533051],
           [1.62534646],
           [1.62536107],
           [1.62537445],
           [1.6253867 ],
           [1.62539792],
           [1.6254082 ]])

## Model ignoring climate change

```python
ss2 = sm.Eq(k,(s*(((k)**alpha))+(1-delta)*k)/((1+n)*(1+g)))
ss2
```

Create a vector to plug in for time, t

```python
y_vec = np.zeros((100,1))
```

```python
y_vec[0] = 1
```

plug vector y_vec in the equation for behavior of capital stock per unit of effective labour.

```python
for i in range(1, 100):
    y_vec[i] = (s*y_vec[i-1]**alpha+(1-delta)*y_vec[i-1])/((1+g)*(1+n))
```

### Plot both functions in the same graph

```python
fid, ax = plt.subplots()
ax.plot(x_vec, label="model including climate change")
ax.plot(y_vec, label="model ignoring climate change")
ax.set(xlabel = 'time, t', ylabel ='k(t)')
plt.title('capital stock per unit of effective labour over time')
plt.legend()
plt.show()
```

Both equations converge to a steady state over time. The model including climate change has a lower steady state because it implements climate change as a external cost.

# Steady state calculations

We now want to find an **analytic an analytical** expression for the steady state, that is: where $ k_{t+1} = k_{t} =k_{ss}$

```python

k = sm.symbols('k')
alpha = sm.symbols('alpha')
delta = sm.symbols('delta')
s = sm.symbols('s')
A = sm.symbols('A')
D = sm.symbols('D')
n = sm.symbols('n')
g = sm.symbols('g')
```

```python
sm.init_printing(use_unicode=True)

f = (1-D)*((k)**alpha)
ss = sm.Eq(k,(s*f+(1-delta)*k)/((1+n)*(1+g)))
kss = sm.solve(ss,k)[0]
kss

```

In the standard Solow model, **without the damage-function**, the steady state capital is given by:

```python
f1= (k)**alpha
ss1 = sm.Eq(k,(s*f1+(1-delta)*k)/((1+n)*(1+g)))
kss1 = sm.solve(ss1,k)[0]
kss1
```

From these equations it becomes clear that, with D≤1, the steady state capital growth is reduced by including the damage-function, compared to the the model without

We can also find the **specific numerical value** for steady state capital. Usning the same parameter values as before, we have

$$s=0.2 $$
$$ g=0.02 $$
$$ n=0.01 $$
$$ delta=0.1$$
$$ alpha=1/3 $$
$$ D=0,1 $$

```python
ss_func = sm.lambdify((s,g,n,delta,alpha,D),kss)
```

Steady state capital, including the damage function, is then given by:

```python
ss_func(0.2,0.02,0.01,0.1,1/3, 0.1)
```

By comparison, excluding the damage function, the steady state is given by:

```python
ss_func(0.2,0.02,0.01,0.1,1/3, 0)
```

Notice how these values corresponds to steady state levels the figure.

It becomes clear that the steady state capital excluding the damage function (representing climate change) is higher than the steady state capital in case of climate damage. Climate change is a negative externality for the society. For example, increased temperature causes sea level to rise and drought periods. This again, can increase human conflicts. Today's generation benefits from the use of fossil fuels, which releases greenhouse gases into the atmosphere and in turn causes climate change. But the benefit from using fossil fuels, does not internalize these external costs of climate change.

# Impact of damage on steady state capital

In order to visualize the impact of damage due to climate change (ceteris paribus), we plot the steady state as a dependent variable of D.

First, create a list that contains all values we want D to take

```python
Ds = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9]
```

Define the function that returns the steady state only dependent on D.

```python
def calc_ss(D):
    return ss_func(0.2,0.02,0.01,0.1,1/3, D)

```

Create a while loop so that a new list (nDs)is generated that contains all steady states for the respective D.

```python
index = 0
while index <= len(Ds)-1:
    Ds[index] = calc_ss(Ds[index])
    index = index +1

print(Ds)
```

    [1.903831539231319, 1.6255198652134988, 1.3622709566173723, 1.1150018222907796, 0.8848209414512797, 0.6731060958136442, 0.4816355156188146, 0.31283144391803264, 0.17028386957717148, 0.06020443945235181]

```python
Damage = [0,0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9]
```

## Plot:

```python
fid, ax = plt.subplots()
ax.plot(Damage, Ds)
ax.set(xlabel = 'Damage', ylabel ='Steady state')
plt.show()
```

From the plot we can see that as damage increases, the steady state capital decreases. This could be due to higher expenses in order to compensate for i.e. agricultural damage.

```python

```

```python

```