# CS4277/CS5477 Lab 3-2: Absolute Pose Estimation

### Introduction
In this assignment, you will get to estimate the rotation and translation of a camera by using the linear n-point camera pose determination algorithm. As discussed in the lecture, You will need at least four 2d-to-3d correspondencs to get a unique solution. We will provide ten 2d-to-3d correspondences and the camera intrinsics in the dataset.

This assignment is worth **10%** of the final grade.

References:
* Lecture 7

Optional references:
* Long Quan, Zhong-Dan Lan. Linear N-point Camera Pose Determination.

### Instructions
This workbook provides the instructions for the assignment, and facilitates the running of your code and visualization of the results. For each part of the assignment, you are required to **complete the implementations of certain functions in the accompanying python file** (`pnp.py`).

To facilitate implementation and grading, all your work is to be done in that file, and **you only have to submit the .py file**.

Please note the following:
1. Fill in your name, email, and NUSNET ID at the top of the python file.
2. The parts you need to implement are clearly marked with the following:

    ```
    """ YOUR CODE STARTS HERE """

    """ YOUR CODE ENDS HERE """
    ```

    , and you should write your code in between the above two lines.
3. Note that for each part, there may certain functions that are prohibited to be used. It is important **NOT to use those prohibited functions** (or other functions with similar functionality). If you are unsure whether a particular function is allowed, feel free to ask any of the TAs.

### Submission Instructions
Zip your completed `pnp.py` and `eight_point.py` and upload onto the relevant work bin in Luminus.

---
## Part 1: Absolute Pose Estimation

In this part, you will implement the linear n-point camera pose determination algorithm. You will estimate the camera postion and orientation given a calibrated camera and ten 2d-to-3d correspondences. Each pair of correspondences $\mathbf{p}_i \leftrightarrow \mathbf{u}_i$ and $\mathbf{p}_j \leftrightarrow \mathbf{u}_j$ gives a constraint on the unknown camera-point distances:
$$
d_{ij}^2 = x_i^2 + x_j^2 -2x_ix_jcos\theta_{ij},
$$
where $d_{ij} = \|\mathbf{p}_i - \mathbf{p}_j\|$ and $\theta_{ij}$ is the inter-point distance and angle. The quadratic constraint can be written as :
$$
f_{ij}(x_i, x_j) = x_i^2 + x_j^2 -2x_ix_j\cos\theta_{ij}-d_{ij}^2 = 0.
$$
For $n=3$, we can obtain three constraints
$$
\begin{cases}
f_{12}(x_1, x_2) = 0 \\
f_{13}(x_1, x_3) = 0 \\
f_{23}(x_2, x_3) = 0
\end{cases}
$$
for the three unknown distances $x_1, x_2, x_3$. The elimination of $x_2, x_3$ gives an eighth degree polynomial in $x_1$:
$$
g(x) = a_5x^4 + a_4x^3 + a_3x^2 + a_2x + a_1 = 0,
$$
where $x = x_1^2$. Thus, given ten 2d-to-3d correspondences in the dataset, you will get $\frac{9 \times 8} {2} = 36  $ constraints for each unknown $x_i$. The matrix equation can be written as:
$$
\begin{bmatrix}
a_1 & a_2 & a_3 & a_4 & a_5 \\
a_1^2 & a_2^2 & a_3^2 & a_4^2 & a_5^2 \\
\vdots & \vdots & \vdots & \vdots & \vdots \\
a_1^{36} & a_2^{36} & a_3^{36} & a_4^{36} & a_5^{36}
\end{bmatrix}
\begin{bmatrix}
1 \\
x \\
x_2 \\
x^3 \\
x^4
\end{bmatrix} = \mathbf{A}_{36 \times 5}\mathbf{t}_5 = 0.
$$
The vector $\mathbf{t}_5$ is obtained from the singular value decomposition of $\mathbf{A}_{36 \times 5}$. Then $x$  can be calculated as :
$$
x = \text{average}(t_1/t_0, t_2/t_1, t_3/t_2, t_4/t_3),
$$
and the final depth is $x_i = \sqrt x$. We will repeat the same process for all other points.

Here are some steps you will follow during the implementation:
1. Construct the matrix $\mathbf{A}$, which is made up of the coefficients of the polynomial. We provide a help function `extract_coeff()` to extract the coefficients of a polynomial. An example of how to use the function is given below. Note that you will compute the `cos_theta12, cos_theta23, cos_theta_13` and `d12, d23, d13` using the real data.
2. Compute the camera-point distance $x_i$ for each point by taking SVD of matrix $\mathbf{A}$ .
3. Reconsruct the 3d coordinates of each point by using the helper function `reconstruct_3d()`. Note that the 2d points should be in the homogeneous coordinate in this function.
4. Recover the camera rotationa and translation by using the ICP algorithm. We provid the helper function `icp()`,where the inputs are the 3d coordinates of all points under the world and camera coordinates. Note that you may need the `np.squeeze()` to convert the data into the required format.

After you get the rotation and translation of the camera, you can check your results by reprojecting all 3d points into image space and compare with the ground truth. You will find that the reprojections of the 3d points are close to the ground truth pixels if your estimations are correct (As shown below).

**Implement the following function(s): `cv2.solvePnP()`**
* <u>You may use the following functions</u>:
  `np.linalg.svd()`, `np.linalg.inv()`, `combinations()`,

```python
%load_ext autoreload
%autoreload 1

%aimport pnp
%matplotlib inline
%load_ext autoreload
import numpy as np
import sympy as sym
import scipy.io as sio
from sympy.polys import subresultants_qq_zz
from itertools import combinations
import cv2
import matplotlib.pyplot as plt
from pnp import pnp_algo, visualize,extract_coeff
np.set_printoptions(precision=6)
```

    The autoreload extension is already loaded. To reload it, use:
      %reload_ext autoreload

```python
x1, x2, x3 = sym.symbols('x1, x2, x3')
cos_theta12, cos_theta23, cos_theta13 = 0.0, 0.0, 0.0
d12, d23, d13 = 0.0, 0.0, 0.0
a = extract_coeff(x1, x2, x3, cos_theta12, cos_theta23, cos_theta13, d12, d23, d13)
```

```python
data = sio.loadmat('data/data_pnp.mat')
points2d = data['points2d']
points3d = data['points3d']
K = data['k']
r, t = pnp_algo(K, points2d, points3d)
points2d = np.squeeze(points2d)
points3d = np.squeeze(points3d)
visualize(r, t, points3d, points2d, K)
```

```python

```

```python

```


```python
# goal: have sympy do the mechanical substitutions, to double-check the desired relations
# once this is done, this will also make it easier for a human to check (just double-check the definitions), and easier
# to check for arbitrary splittings

from sympy import *
from sympy import init_printing
init_printing()
```

```python
symbolic_traj = dict()
symbolic_traj['x_0'] = symbols('x_0')
symbolic_traj['v_0'] = symbols('v_0')
dt = symbols('dt')
m = symbols('m')
gamma = symbols('gamma')
f = symbols('f')
kT = symbols('kT')
U = symbols('U')

def count_steps(splitting="OVRVO"):
    n_O = sum([step == "O" for step in splitting])
    n_R = sum([step == "R" for step in splitting])
    n_V = sum([step == "V" for step in splitting])
    return n_O, n_R, n_V

def create_variable_names(i):
    name_x = 'x_{}'.format(i)
    name_v = 'v_{}'.format(i)

    symbolic_traj[name_x] = symbols(name_x)
    symbolic_traj[name_v] = symbols(name_v)

    x = symbolic_traj['x_{}'.format(i-1)]
    v = symbolic_traj['v_{}'.format(i-1)]

    return x, v, name_x, name_v

def apply_R(i, h):
    x, v, name_x, name_v = create_variable_names(i)

    symbolic_traj[name_x] = x + v * h
    symbolic_traj[name_v] = v

def apply_V(i, h):
    x, v, name_x, name_v = create_variable_names(i)

    symbolic_traj[name_x] = x
    symbolic_traj[name_v] = v + f(x) * h / m

def apply_O(i, h):
    a = exp(-gamma * h)
    b = sqrt(1 - exp(-2 * gamma * h))

    x, v, name_x, name_v = create_variable_names(i)

    symbolic_traj[name_x] = x
    symbolic_traj[name_v] = a * v + b * sqrt(kT / m) * symbols('xi_{}'.format(i))
    # xi_i is an i.i.d. standard normal r.v.

def total_energy(x, v):
    return U(x) + (0.5 * m * v**2)

def get_total_energy_at_step(i):
    x = symbolic_traj['x_{}'.format(i)]
    v = symbolic_traj['v_{}'.format(i)]
    return total_energy(x, v)

def apply_integrator(i, splitting="OVRVO"):
    heat, W_shad = 0, 0

    n_O, n_R, n_V = count_steps(splitting)

    for step in splitting:
        if step == "O":
            apply_O(i, dt / n_O)
            heat += get_total_energy_at_step(i) - get_total_energy_at_step(i-1)
        elif step == "R":
            apply_R(i, dt / n_R)
            W_shad += get_total_energy_at_step(i) - get_total_energy_at_step(i-1)
        elif step == "V":
            apply_V(i, dt / n_V)
            W_shad += get_total_energy_at_step(i) - get_total_energy_at_step(i-1)
        i += 1

    return heat, W_shad

splitting = "OVRVO"
heat, W_shad = apply_integrator(1, splitting)
delta_E = get_total_energy_at_step(len(splitting)) - get_total_energy_at_step(0)

delta_E
```

```python
W_shad
```

```python
heat
```

```python
delta_E - (W_shad + heat)
```

```python
# also define path probabilities?

# here, let's define the individual step probabilities
def normal_pdf(x, mu, sigma):
    return (1 / (sqrt(2 * pi * sigma**2))) * exp(-(x - mu)**2 / (2 * sigma**2))

def R_prob(from_x, from_v, to_x, to_v, h):
    '''deterministic position update'''
    if (from_v == to_v) and (to_x == (from_x + from_v * h)):
        return 1
    else:
        return 0

def V_prob(from_x, from_v, to_x, to_v, h):
    if (from_x == to_x) and (to_v == (from_v + f(from_x) * h / m)):
        return 1
    else:
        return 0

def O_prob(from_x, from_v, to_x, to_v, h):
    ''' need to double check this, probably dropped a 2 somewhere...'''
    a = exp(-gamma * h) # okay
    b = sqrt(1 - exp(-2 * gamma * h)) # okay

    sigma = b * sqrt(kT / m) # double-check this! is this the definition of sigma or sigma^2?
    mu = a * from_v # okay

    if (from_x == to_x):
        return normal_pdf(to_v, mu, sigma)
    else:
        return 0

# for example, what's the probability of a small v perturbation
O_prob(0, 0, 0, 0.1, dt / 2)
```

```python
O_prob(symbolic_traj['x_0'], symbolic_traj['v_0'], symbolic_traj['x_1'], symbolic_traj['v_1'], dt / 2)
```

```python
def forward_path_probability(splitting="OVRVO"):
    path_prob = 1
    n_O, n_R, n_V = count_steps(splitting)

    for i, step in enumerate(splitting):
        x_0, v_0 = symbolic_traj['x_{}'.format(i)], symbolic_traj['v_{}'.format(i)]
        x_1, v_1 = symbolic_traj['x_{}'.format(i+1)], symbolic_traj['v_{}'.format(i+1)]

        if step == "O":
            step_prob = O_prob(x_0, v_0, x_1, v_1,
                               dt / n_O)
        elif step == "R":
            step_prob = R_prob(x_0, v_0, x_1, v_1,
                               dt / n_R)
        elif step == "V":
            step_prob = V_prob(x_0, v_0, x_1, v_1,
                               dt / n_V)

        path_prob = path_prob * step_prob
    return path_prob
```

```python
forward_path_probability("OVRVO")
```

```python
def reverse_path_probability(splitting="OVRVO"):
    '''same as above, just reverse order of trajectory and steps appropriately?'''

    path_prob = 1
    n_O, n_R, n_V = count_steps(splitting)

    for i in range(len(splitting))[::-1]:
        step = splitting[i]

        x_0, v_0 = symbolic_traj['x_{}'.format(i+1)], symbolic_traj['v_{}'.format(i+1)]
        x_1, v_1 = symbolic_traj['x_{}'.format(i)], symbolic_traj['v_{}'.format(i)]

        if step == "O":
            step_prob = O_prob(x_0, v_0, x_1, v_1,
                               dt / n_O)
        elif step == "R":
            step_prob = R_prob(x_0, v_0, x_1, v_1,
                               dt / n_R)
        elif step == "V":
            step_prob = V_prob(x_0, v_0, x_1, v_1,
                               dt / n_V)

        path_prob = path_prob * step_prob
    return path_prob
```

```python
def CFT_definition_of_work(splitting="OVRVO"):
    return - ln(reverse_path_probability(splitting) / forward_path_probability(splitting))
```

```python
CFT_definition_of_work("OVRVO")
```

```python
reverse_path_probability()
```

```python
forward_path_probability()
```

```python
# hmm that doesn't look quite right
```

Using sympy to compute relative path action.ipynb


This notebook is part of https://github.com/AudioSceneDescriptionFormat/splines, see also https://splines.readthedocs.io/.

[back to overview](natural.ipynb)

# Non-Uniform Natural Splines

The derivation is similar to
[the uniform case](natural-uniform.ipynb),
but this time the parameter intervals can have arbitrary values.

```python
import sympy as sp
sp.init_printing(order='grevlex')
```

```python
from utility import NamedExpression
```

```python
t = sp.symbols('t')
```

Just like in the uniform case,
we are considering two adjacent spline segments,
but this time we must allow arbitrary parameter values:

```python
t3, t4, t5 = sp.symbols('t3:6')
```

```python
b_monomial = sp.Matrix([t**3, t**2, t, 1]).T
b_monomial
```

```python
coefficients3 = sp.symbols('a:dbm3')[::-1]
coefficients4 = sp.symbols('a:dbm4')[::-1]
```

```python
b_monomial.dot(coefficients3)
```

```python
p3 = NamedExpression(
    'pbm3',
    b_monomial.dot(coefficients3).subs(t, (t - t3)/(t4 - t3)))
p4 = NamedExpression(
    'pbm4',
    b_monomial.dot(coefficients4).subs(t, (t - t4)/(t5 - t4)))
display(p3, p4)
```

```python
pd3 = p3.diff(t)
pd4 = p4.diff(t)
display(pd3, pd4)
```

```python
equations = [
    p3.evaluated_at(t, t3).with_name('xbm3'),
    p3.evaluated_at(t, t4).with_name('xbm4'),
    p4.evaluated_at(t, t4).with_name('xbm4'),
    p4.evaluated_at(t, t5).with_name('xbm5'),
    pd3.evaluated_at(t, t3).with_name('xbmdot3'),
    pd3.evaluated_at(t, t4).with_name('xbmdot4'),
    pd4.evaluated_at(t, t4).with_name('xbmdot4'),
    pd4.evaluated_at(t, t5).with_name('xbmdot5'),
]
```

We introduce a few new symbols to simplify the display,
but we keep calculating with $t_i$:

```python
deltas = {
    t3: 0,
    t4: sp.Symbol('Delta3'),
    t5: sp.Symbol('Delta3') + sp.Symbol('Delta4'),
}
```

```python
for e in equations:
    display(e.subs(deltas))
```

```python
coefficients = sp.solve(equations, coefficients3 + coefficients4)
```

```python
for c, e in coefficients.items():
    display(NamedExpression(c, e.subs(deltas)))
```

```python
pdd3 = pd3.diff(t)
pdd4 = pd4.diff(t)
display(pdd3, pdd4)
```

```python
sp.Eq(pdd3.expr.subs(t, t4), pdd4.expr.subs(t, t4))
```

```python
_.subs(coefficients).subs(deltas).simplify()
```

Like in the uniform case,
we can generalize by renaming index $4$ to $i$:

\begin{equation*}
\frac{1}{\Delta_{i-1}} \dot{\boldsymbol{x}}_{i-1}
+
\left(\frac{2}{\Delta_{i-1}} + \frac{2}{\Delta_i}\right)
\dot{\boldsymbol{x}}_i
+
\frac{1}{\Delta_i} \dot{\boldsymbol{x}}_{i+1}
=
\frac{3 (\boldsymbol{x}_i - \boldsymbol{x}_{i-1})}{{\Delta_{i-1}}^2} +
\frac{3 (\boldsymbol{x}_{i+1} - \boldsymbol{x}_i)}{{\Delta_i}^2}
\end{equation*}

We are not showing the full matrix here,
because it would be quite a bit more complicated and less helpful than
in the uniform case.

## End Conditions

Like in the [uniform case](natural-uniform.ipynb#End-Conditions),
we can come up with a few end conditions
in order to define the missing matrix rows.

The Python class [splines.Natural](../python-module/splines.rst#splines.Natural)
uses "natural" end conditions by default.

"Natural" end conditions are derived in
[a separate notebook](end-conditions-natural.ipynb),
yielding these expressions:

\begin{align*}
2 \Delta_0 \dot{\boldsymbol{x}}_0
+
\Delta_0 \dot{\boldsymbol{x}}_1
&=
3 (\boldsymbol{x}_1 - \boldsymbol{x}_0)
\\
\Delta_{N-2} \dot{\boldsymbol{x}}_{N-2}
+
2 \Delta_{N-2} \dot{\boldsymbol{x}}_{N-1}
&=
3 (\boldsymbol{x}_{N-1} - \boldsymbol{x}_{N-2})
\end{align*}

Other end conditions can be derived as shown in
[the notebook about uniform "natural" splines](natural-uniform.ipynb#End-Conditions).


```python
%matplotlib inline
%config InlineBackend.figure_format = 'retina'

%load_ext autoreload
%autoreload 2

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.pylab as pylab
from simplified_monorotor import Monorotor
import plotting
import testing
import trajectories

pylab.rcParams['figure.figsize'] = 10,10
```

# PID controller

#### TODO - implement PID controller

Implement the PD Controller math in `thrust_control`:

$$
\begin{align}
e &= z_{\text{target}} - z_{\text{actual}} \\
\dot{e} &= \dot{z}_{\text{target}} - \dot{z}_{\text{actual}} \\
\bar{u}_1 &= k_p e + k_d \dot{e} + k_i\int_0^te(t')dt' +\ddot{z}_{\text{ff}} \\
u_1 &= m(g - \bar{u}_1)
\end{align}
$$

> **Note**: we've introduced `integrated_error` and set it to 0.0 at initialization. We also now need to know how much time `dt` has elapsed between successive calls to `thrust_control` as this influences the integrated error.

```python
class PIDController:

    def __init__(self, k_p, k_d, k_i, m):
        self.k_p = k_p
        self.k_d = k_d
        self.k_i = k_i
        self.vehicle_mass = m
        self.g = 9.81
        self.integrated_error = 0.0

    def thrust_control(self,
                z_target,
                z_actual,
                z_dot_target,
                z_dot_actual,
                dt=0.1,
                z_dot_dot_ff=0.0):
        #
        # TODO
        #   implement a PID controller according to the math
        #   shown above. Remember that the integral accumulates
        #   error by adding "rectangles" of error with area
        #   equal to err * dt
        #
        #   don't forget to return a thrust command
        #
        e = z_target - z_actual
        ed = z_dot_target - z_dot_actual
        self.integrated_error += e * dt

        u_hat = self.k_p * e + self.k_d * ed + self.k_i * self.integrated_error + z_dot_dot_ff

        return self.vehicle_mass * (self.g - u_hat)

testing.pid_controller_test(PIDController)
```

    Tests pass

#### TODO 2 - Adjust gains
Run the simulation code below with `K_I = 0.0` to remind yourself what a PD controller does when there's a large mass_error (remember, a PID controller with zero integral gain is just a PD controller).

Then slowly increase `K_I` and rerun the cell. Note what happens to the error.

_Hint_: higher values of K_i help reduce the bias faster.

```python
MASS_ERROR = 1.5
K_P = 120.0
K_D = 25.0
K_I = 8.0 # TODO - increase to 0.5, 1, 2, etc...

# preparation
drone = Monorotor()
perceived_mass = drone.m * MASS_ERROR
controller    = PIDController(K_P, K_D, K_I, perceived_mass)

t, z_path, z_dot_path = trajectories.step(duration=10.0)

dt = t[1] - t[0]

# run simulation
history = []
for z_target, z_dot_target in zip(z_path, z_dot_path):
    z_actual = drone.z
    z_dot_actual = drone.z_dot

    u = controller.thrust_control(z_target, z_actual,
                                  z_dot_target, z_dot_actual,
                                  dt)

    drone.thrust = u
    drone.advance_state(dt)
    history.append(drone.X)

# generate plots
z_actual = [h[0] for h in history]
plotting.compare_planned_to_actual(z_actual, z_path, t)
```

Hopefully you were able to adjust K_P, K_D, and K_I so that the drone gets to the target altitude (even with a large mass error) pretty quickly.

If so, congratulations! You now understand PID control!

[Solution](/notebooks/PID%20Controller%20Solution.ipynb)