(FCD)=

# 1.5 Definición de función, continuidad y derivada

```{admonition} Notas para contenedor de docker:

Comando de docker para ejecución de la nota de forma local:

nota: cambiar `<ruta a mi directorio>` por la ruta de directorio que se desea mapear a `/datos` dentro del contenedor de docker y `<versión imagen de docker>` por la versión más actualizada que se presenta en la documentación.

`docker run --rm -v <ruta a mi directorio>:/datos --name jupyterlab_optimizacion -p 8888:8888 -d palmoreck/jupyterlab_optimizacion:<versión imagen de docker>`

password para jupyterlab: `qwerty`

Detener el contenedor de docker:

`docker stop jupyterlab_optimizacion`

Documentación de la imagen de docker `palmoreck/jupyterlab_optimizacion:<versión imagen de docker>` en [liga](https://github.com/palmoreck/dockerfiles/tree/master/jupyterlab/optimizacion).

```

---

Nota generada a partir de la [liga1](https://www.dropbox.com/s/jfrxanjls8kndjp/Diferenciacion_e_Integracion.pdf?dl=0), [liga2](https://www.dropbox.com/s/mmd1uzvwhdwsyiu/4.3.2.Teoria_de_convexidad_Funciones_convexas.pdf?dl=0) e inicio de [liga3](https://www.dropbox.com/s/ko86cce1olbtsbk/4.3.1.Teoria_de_convexidad_Conjuntos_convexos.pdf?dl=0).

```{admonition} Al final de esta nota la comunidad lectora:
:class: tip

* Aprenderá las definiciones de función y derivada de una función en algunos casos de interés para el curso. En específico el caso de derivada direccional es muy importante.

* Aprenderá que el gradiente y Hessiana de una función son un vector y una matriz de primeras (información de primer orden) y segundas derivadas (información de segundo orden) respectivamente.

* Aprenderá algunas fórmulas utilizadas con el operador nabla de diferenciación.

* Aprenderá la diferencia entre el cálculo algebraico o simbólico y el numérico vía el paquete *SymPy*.

```

## Función

```{admonition} Definición

Una función, $f$, es una regla de correspondencia entre un conjunto nombrado dominio y otro conjunto nombrado codominio.
```

### Notación

$f: A \rightarrow B$ es una función de un conjunto $\text{dom}f \subseteq A$ en un conjunto $B$.

```{admonition} Observación
:class: tip

$\text{dom}f$ (el dominio de $f$) puede ser un subconjunto propio de $A$, esto es, algunos elementos de $A$ y otros no, son mapeados a elementos de $B$.
```

En lo que sigue se considera al espacio $\mathbb{R}^n$ y se asume que conjuntos y subconjuntos están en este espacio.

(CACCI)=

### Conjunto abierto, cerrado, cerradura e interior

```{margin}

Un punto $x$ se nombra **punto límite** de un conjunto $X$, si existe una sucesión $\{x_k\} \subset X$ que converge a $x$. El conjunto de puntos límites se nombra **cerradura** o *closure* de $X$ y se denota como $\text{cl}X$.

Un conjunto $X$ se nombra **cerrado** si es igual a su cerradura.

```

```{admonition} Definición

El interior de un conjunto $X$ es el conjunto de **puntos interiores**: un punto $x$ de un conjunto $X$ se llama interior si existe una **vecindad** de $x$ (conjunto abierto\* que contiene a $x$) contenida en $X$.

\*Un conjunto $X$ se dice que es **abierto** si $\forall x \in X$ existe una bola abierta\* centrada en $x$ y contenida en $X$. Es equivalente escribir que $X$ es **abierto** si su complemento $\mathbb{R}^n \ X$ es cerrado.

\*Una **bola abierta** con radio $\epsilon>0$ y centrada en $x$ es el conjunto: $B_\epsilon(x) =\{y \in \mathbb{R}^n : ||y-x|| < \epsilon\}$. Ver {ref}`Ejemplos de gráficas de normas en el plano <EGNP>` para ejemplos de bolas abiertas en el plano.
```

En lo siguiente $\text{intdom}f$ es el **interior** del dominio de $f$.

## Continuidad

```{admonition} Definición

$f: \mathbb{R}^n \rightarrow \mathbb{R}^m$ es continua en $x \in \text{dom}f$ si $\forall \epsilon >0 \exists \delta > 0$ tal que:

$$y \in \text{dom}f, ||y-x||_2 \leq \delta \implies ||f(y)-f(x)||_2 \leq \epsilon$$

```

```{admonition} Comentarios

* $f$ continua en un punto $x$ del dominio de $f$ entonces $f(y)$ es arbitrariamente cercana a $f(x)$ para $y$ en el dominio de $f$ cercana a $x$.

* Otra forma de definir que $f$ sea continua en $x \in \text{dom}f$ es con sucesiones y límites: si $\{x_i\}_{i \in \mathbb{N}} \subseteq \text{dom}f$ es una sucesión de puntos en el dominio de $f$ que converge a $x \in \text{dom}f$, $\displaystyle \lim_{i \rightarrow \infty}x_i = x$, y $f$ es continua en $x$ entonces la sucesión $\{f(x_i)\}_{i \in \mathbb{N}}$ converge a $f(x)$: $\displaystyle \lim_{i \rightarrow \infty}f(x_i) = f(x) = f \left(\displaystyle \lim_{i \rightarrow \infty} x_i \right )$.
```

### Notación

$\mathcal{C}([a,b])=\{\text{funciones } f:\mathbb{R} \rightarrow \mathbb{R} \text{ continuas en el intervalo [a,b]}\}$ y $\mathcal{C}(\text{dom}f) = \{\text{funciones } f:\mathbb{R}^n \rightarrow \mathbb{R}^m \text{ continuas en su dominio}\}$.

## Función Diferenciable

### Caso $f: \mathbb{R} \rightarrow \mathbb{R}$

```{admonition} Definición

$f$ es diferenciable en $x_0 \in (a,b)$ si $\displaystyle \lim_{x \rightarrow x_0} \frac{f(x)-f(x_0)}{x-x_0}$ existe y escribimos:

$$f^{(1)}(x_0) = \displaystyle \lim_{x \rightarrow x_0} \frac{f(x)-f(x_0)}{x-x_0}.$$
```

$f$ es diferenciable en $[a,b]$ si es diferenciable en cada punto de $[a,b]$. Análogamente definiendo la variable $h=x-x_0$ se tiene:

$f^{(1)}(x_0) = \displaystyle \lim_{h \rightarrow 0} \frac{f(x_0+h)-f(x_0)}{h}$ que típicamente se escribe como:

$$f^{(1)}(x) = \displaystyle \lim_{h \rightarrow 0} \frac{f(x+h)-f(x)}{h}.$$

```{admonition} Comentario

Si $f$ es diferenciable en $x_0$ entonces $f(x) \approx f(x_0) + f^{(1)}(x_0)(x-x_0)$. Gráficamente:

```

Como las derivadas también son funciones tenemos una notación para las derivadas que son continuas:

### Notación

$\mathcal{C}^n([a,b])=\{\text{funciones } f:\mathbb{R} \rightarrow \mathbb{R} \text{ con } n \text{ derivadas continuas en el intervalo [a,b]}\}$.

En Python podemos utilizar el paquete [SymPy](https://www.sympy.org/en/index.html) para calcular límites y derivadas de forma **simbólica** (ver [sympy/calculus](https://docs.sympy.org/latest/tutorial/calculus.html)) que es diferente al cálculo **numérico** que se revisa en {ref}`Polinomios de Taylor y diferenciación numérica <PTDN>`.

### Ejemplo

```python
import sympy
```

**Límite de $\frac{\cos(x+h) - \cos(x)}{h}$ para $h \rightarrow 0$:**

```python
x, h = sympy.symbols("x, h")
```

```python
quotient = (sympy.cos(x+h) - sympy.cos(x))/h
```

```python
sympy.pprint(sympy.limit(quotient, h, 0))
```

    -sin(x)

Lo anterior corresponde a la **derivada de $\cos(x)$**:

```python
x = sympy.Symbol('x')
```

```python
sympy.pprint(sympy.cos(x).diff(x))
```

    -sin(x)

**Si queremos evaluar la derivada podemos usar:**

```python
sympy.pprint(sympy.cos(x).diff(x).subs(x,sympy.pi/2))
```

    -1

```python
sympy.pprint(sympy.Derivative(sympy.cos(x), x))
```

    d
    ──(cos(x))
    dx

```python
sympy.pprint(sympy.Derivative(sympy.cos(x), x).doit_numerically(sympy.pi/2))
```

    -1.00000000000000

### Caso $f: \mathbb{R}^n \rightarrow \mathbb{R}^m$

```{admonition} Definición

$f$ es diferenciable en $x \in \text{intdom}f$ si existe una matriz $Df(x) \in \mathbb{R}^{m\times n}$ tal que:

$$\displaystyle \lim_{z \rightarrow x, z \neq x} \frac{||f(z)-f(x)-Df(x)(z-x)||_2}{||z-x||_2} = 0, z \in \text{dom}f$$

en este caso $Df(x)$ se llama la derivada de $f$ en $x$.
```

```{admonition} Observación
:class: tip

Sólo puede existir a lo más una matriz que satisfaga el límite anterior.
```

```{margin}

Una función afín es de la forma $h(x) = Ax+b$ con $A \in \mathbb{R}^{p \times n}$ y $b \in \mathbb{R}^p$. Ver [Affine_transformation](https://en.wikipedia.org/wiki/Affine_transformation)

```

```{admonition} Comentarios:

* $Df(x)$ también es llamada la **Jacobiana** de $f$.

* Se dice que $f$ es diferenciable si $\text{dom}f$ es abierto y es diferenciable en cada punto de $\text{dom}f.$

* La función: $f(x) + Df(x)(z-x)$ es afín y se le llama **aproximación de orden $1$** de $f$ en $x$ (o también cerca de $x$). Para $z$ cercana a $x$ ésta aproximación es cercana a $f(z)$.

* $Df(x)$ puede encontrarse con la definición de límite anterior o con las derivadas parciales: $Df(x)_{ij} = \frac{\partial f_i(x)}{\partial x_j}, i=1,\dots,m, j=1,\dots,n$ definidas como:

$$\frac{\partial f_i(x)}{\partial x_j} = \displaystyle \lim_{h \rightarrow 0} \frac{f_i(x+he_j)-f_i(x)}{h}$$

donde: $f_i : \mathbb{R}^n \rightarrow \mathbb{R}$, $i=1,\dots,m,j=1,\dots,n$ y $e_j$ $j$-ésimo vector canónico que tiene un número $1$ en la posición $j$ y $0$ en las entradas restantes.

* Si $f: \mathbb{R}^n \rightarrow \mathbb{R}, Df(x) \in \mathbb{R}^{1\times n}$, su transpuesta se llama **gradiente**, se denota $\nabla f(x)$, es una función $\nabla f : \mathbb{R}^n \rightarrow \mathbb{R}^n$, recibe un vector y devuelve un vector columna y sus componentes son derivadas parciales:

$$\nabla f(x) = Df(x)^T =
    \left[ \begin{array}{c}
    \frac{\partial f(x)}{\partial x_1}\\
    \vdots\\
    \frac{\partial f(x)}{\partial x_n}
    \end{array}
    \right] = \left[
    \begin{array}{c}
    \displaystyle \lim_{h \rightarrow 0} \frac{f(x+he_1) - f(x)}{h}\\
    \vdots\\
    \displaystyle \lim_{h \rightarrow 0} \frac{f(x+he_n) - f(x)}{h}
    \end{array}
    \right] \in \mathbb{R}^{n\times 1}.$$

* En este contexto, la aproximación de primer orden a $f$ en $x$ es: $f(x) + \nabla f(x)^T(z-x)$ para $z$ cercana a $x$.
```

### Notación

$\mathcal{C}^n(\text{dom}f) = \{\text{funciones } f:\mathbb{R}^n \rightarrow \mathbb{R}^m \text{ con } n \text{ derivadas continuas en su dominio}\}$.

### Ejemplo

$f : \mathbb{R}^2 \rightarrow \mathbb{R}^2$ dada por:

$$f(x) =
\left [
\begin{array}{c}
x_1x_2 + x_2^2\\
x_1^2 + 2x_1x_2 + x_2^2\\
\end{array}
\right ]
$$

con $x = (x_1, x_2)^T$. Calcular la derivada de $f$.

```python
x1, x2 = sympy.symbols("x1, x2")
```

**Definimos funciones $f_1, f_2$ que son componentes del vector $f(x)$**.

```python
f1 = x1*x2 + x2**2
```

```python
sympy.pprint(f1)
```

              2
    x₁⋅x₂ + x₂

```python
f2 = x1**2 + x2**2 + 2*x1*x2
```

```python
sympy.pprint(f2)
```

      2               2
    x₁  + 2⋅x₁⋅x₂ + x₂

**Derivadas parciales:**

Para $f_1(x) = x_1x_2 + x_2^2$:

```{margin}

**Derivada parcial de $f_1$ respecto a $x_1$.**
```

```python
df1_x1 = f1.diff(x1)
```

```python
sympy.pprint(df1_x1)
```

    x₂

```{margin}

**Derivada parcial de $f_1$ respecto a $x_2$.**
```

```python
df1_x2 = f1.diff(x2)
```

```python
sympy.pprint(df1_x2)
```

    x₁ + 2⋅x₂

Para $f_2(x) = x_1^2 + 2x_1 x_2 + x_2^2$:

```{margin}

**Derivada parcial de $f_2$ respecto a $x_1$.**
```

```python
df2_x1 = f2.diff(x1)
```

```python
sympy.pprint(df2_x1)
```

    2⋅x₁ + 2⋅x₂

```{margin}

**Derivada parcial de $f_2$ respecto a $x_2$.**
```

```python
df2_x2 = f2.diff(x2)
```

```python
sympy.pprint(df2_x2)
```

    2⋅x₁ + 2⋅x₂

**Entonces la derivada es:**

$$Df(x) =
\left [
\begin{array}{cc}
x_2 & x_1+2x_2\\
2x_1 + 2x_2 & 2x_1+2x_2
\end{array}
\right ]
$$

**Otra opción más fácil es utilizando [Matrices](https://docs.sympy.org/latest/tutorial/matrices.html):**

```python
f = sympy.Matrix([f1, f2])
```

```python
sympy.pprint(f)
```

    ⎡              2    ⎤
    ⎢    x₁⋅x₂ + x₂     ⎥
    ⎢                   ⎥
    ⎢  2               2⎥
    ⎣x₁  + 2⋅x₁⋅x₂ + x₂ ⎦

```{margin}

**Jacobiana de $f$**
```

```python
sympy.pprint(f.jacobian([x1, x2]))
```

    ⎡    x₂        x₁ + 2⋅x₂ ⎤
    ⎢                        ⎥
    ⎣2⋅x₁ + 2⋅x₂  2⋅x₁ + 2⋅x₂⎦

**Para evaluar por ejemplo en $(x_1, x_2)^T = (0, 1)^T$:**

```python
d = f.jacobian([x1, x2])
```

```python
sympy.pprint(d.subs([(x1, 0), (x2, 1)]))
```

    ⎡1  2⎤
    ⎢    ⎥
    ⎣2  2⎦

## Regla de la cadena

```{admonition} Definición

Si $f:\mathbb{R}^n \rightarrow \mathbb{R}^m$ es diferenciable en $x\in \text{intdom}f$ y $g:\mathbb{R}^m \rightarrow \mathbb{R}^p$ es diferenciable en $f(x)\in \text{intdom}g$, se define la composición $h:\mathbb{R}^n \rightarrow \mathbb{R}^p$ por $h(z) = g(f(z))$, la cual es diferenciable en $x$, con derivada:

$$Dh(x)=Dg(f(x))Df(x)\in \mathbb{R}^{p\times n}.$$
```

(EJ1)=

### Ejemplo

Sean $f:\mathbb{R}^n \rightarrow \mathbb{R}$, $g:\mathbb{R} \rightarrow \mathbb{R}$, $h:\mathbb{R}^n \rightarrow \mathbb{R}$ con $h(z) = g(f(z))$ entonces:

$$Dh(x) = Dg(f(x))Df(x) = \frac{dg(f(x))}{dx}\nabla f(x)^T \in \mathbb{R}^{1\times n}$$

y la transpuesta de $Dh(x)$ es: $\nabla h(x) = Dh(x)^T = \frac{dg(f(x))}{dx} \nabla f(x) \in \mathbb{R}^{n\times 1}$.

### Ejemplo

$f(x) = \cos(x), g(x)=\sin(x)$ por lo que $h(x) = \sin(\cos(x))$. Calcular la derivada de $h$.

```python
x = sympy.Symbol('x')
```

```python
f = sympy.cos(x)
```

```python
sympy.pprint(f)
```

    cos(x)

```python
g = sympy.sin(x)
```

```python
sympy.pprint(g)
```

    sin(x)

```python
h = g.subs(x, f)
```

```python
sympy.pprint(h)
```

    sin(cos(x))

```python
sympy.pprint(h.diff(x))
```

    -sin(x)⋅cos(cos(x))

**Otras formas para calcular la derivada de la composición $h$:**

```python
g = sympy.sin
```

```python
h = g(f)
```

```python
sympy.pprint(h.diff(x))
```

    -sin(x)⋅cos(cos(x))

```python
h = sympy.sin(f)
```

```python
sympy.pprint(h.diff(x))
```

    -sin(x)⋅cos(cos(x))

### Ejemplo

$f(x) = x_1 + \frac{1}{x_2}, g(x) = e^x$ por lo que $h(x) = e^{x_1 + \frac{1}{x_2}}$. Calcular la derivada de $h$.

```python
x1, x2 = sympy.symbols("x1, x2")
```

```python
f = x1 + 1/x2
```

```python
sympy.pprint(f)
```

         1
    x₁ + ──
         x₂

```python
g = sympy.exp
```

```python
sympy.pprint(g)
```

    exp

```python
h = g(f)
```

```python
sympy.pprint(h)
```

          1
     x₁ + ──
          x₂
    ℯ

```{margin}

**Derivada parcial de $h$ respecto a $x_1$.**
```

```python
sympy.pprint(h.diff(x1))
```

          1
     x₁ + ──
          x₂
    ℯ

```{margin}

**Derivada parcial de $h$ respecto a $x_2$.**
```

```python
sympy.pprint(h.diff(x2))
```

           1
      x₁ + ──
           x₂
    -ℯ
    ──────────
         2
       x₂

**Otra forma para calcular el gradiente de $h$ (derivada de $h$) es utilizando [how-to-get-the-gradient-and-hessian-sympy](https://stackoverflow.com/questions/39558515/how-to-get-the-gradient-and-hessian-sympy):**

```python
from sympy.tensor.array import derive_by_array
```

```python
sympy.pprint(derive_by_array(h, (x1, x2)))
```

    ⎡                 1  ⎤
    ⎢      1     x₁ + ── ⎥
    ⎢ x₁ + ──         x₂ ⎥
    ⎢      x₂  -ℯ        ⎥
    ⎢ℯ         ──────────⎥
    ⎢               2    ⎥
    ⎣             x₂     ⎦

(CP1)=

### Caso particular

Sean:

* $f: \mathbb{R}^n \rightarrow \mathbb{R}^m$, $f(x) = Ax +b$ con $A \in \mathbb{R}^{m\times n},b \in \mathbb{R}^m$,

* $g:\mathbb{R}^m \rightarrow \mathbb{R}^p$,

* $h: \mathbb{R}^n \rightarrow \mathbb{R}^p$, $h(x)=g(f(x))=g(Ax+b)$ con $\text{dom}h=\{z \in \mathbb{R}^n | Az+b \in \text{dom}g\}$ entonces:

$$Dh(x) = Dg(f(x))Df(x)=Dg(Ax+b)A.$$

```{admonition} Observación
:class: tip

Si $p=1$, $g: \mathbb{R}^m \rightarrow \mathbb{R}$, $h: \mathbb{R}^n \rightarrow \mathbb{R}$ se tiene:

$$\nabla h(x) = Dh(x)^T = A^TDg(Ax+b)^T=A^T\nabla g(Ax+b) \in \mathbb{R}^{n\times 1}.$$

```

(EJRestriccionALinea)=

### Ejemplo

Este caso particular considera un caso importante en el que se tienen funciones restringidas a una línea. Si $f: \mathbb{R}^n \rightarrow \mathbb{R}$, $g: \mathbb{R} \rightarrow \mathbb{R}$ está dada por $g(t) = f(x+tv)$ con $x, v \in \mathbb{R}^n$ y $t \in \mathbb{R}$, entonces escribimos que $g$ es $f$ pero restringida a la línea $x+tv$. La derivada de $g$ es:

$$Dg(t) = \nabla f(x+tv)^T v.$$

El escalar $Dg(0) = \nabla f(x)^Tv$ se llama **derivada direccional** de $f$ en $x$ en la dirección $v$. Un dibujo en el que se considera $\Delta x: = v$:

Como ejemplo considérese $f(x) = x_1 ^2 + x_2^2$ con $x=(x_1, x_2)^T$ y $g(t) = f(x+tv)$ para $v=(v_1, v_2)^T$ vector fijo y $t \in \mathbb{R}$. Calcular $Dg(t)$.

**Primera opción**

```python
x1, x2 = sympy.symbols("x1, x2")
```

```python
f = x1**2 + x2**2
```

```python
sympy.pprint(f)
```

      2     2
    x₁  + x₂

```python
t = sympy.Symbol('t')
v1, v2 = sympy.symbols("v1, v2")
```

```python
new_args_for_f_function = {"x1": x1+t*v1, "x2": x2 + t*v2}
```

```python
g = f.subs(new_args_for_f_function)
```

```python
sympy.pprint(g)
```

               2              2
    (t⋅v₁ + x₁)  + (t⋅v₂ + x₂)

```{margin}

**Derivada de $g$ respecto a $t$: $Dg(t)=\nabla f(x+tv)^T v$.**

```

```python
sympy.pprint(g.diff(t))
```

    2⋅v₁⋅(t⋅v₁ + x₁) + 2⋅v₂⋅(t⋅v₂ + x₂)

**Segunda opción para calcular la derivada utilizando vectores:**

```python
x = sympy.Matrix([x1, x2])
```

```python
sympy.pprint(x)
```

    ⎡x₁⎤
    ⎢  ⎥
    ⎣x₂⎦

```python
v = sympy.Matrix([v1, v2])
```

```python
new_arg_f_function = x+t*v
```

```python
sympy.pprint(new_arg_f_function)
```

    ⎡t⋅v₁ + x₁⎤
    ⎢         ⎥
    ⎣t⋅v₂ + x₂⎦

```python
mapping_for_g_function = {"x1": new_arg_f_function[0],
                          "x2": new_arg_f_function[1]}
```

```python
g = f.subs(mapping_for_g_function)
```

```python
sympy.pprint(g)
```

               2              2
    (t⋅v₁ + x₁)  + (t⋅v₂ + x₂)

```{margin}

**Derivada de $g$ respecto a $t$: $Dg(t)=\nabla f(x+tv)^T v$.**

```

```python
sympy.pprint(g.diff(t))
```

    2⋅v₁⋅(t⋅v₁ + x₁) + 2⋅v₂⋅(t⋅v₂ + x₂)

**Tercera opción definiendo a la función $f$ a partir de $x$ symbol Matrix:**

```python
sympy.pprint(x)
```

    ⎡x₁⎤
    ⎢  ⎥
    ⎣x₂⎦

```python
f = x[0]**2 + x[1]**2
```

```python
sympy.pprint(f)
```

      2     2
    x₁  + x₂

```python
sympy.pprint(new_arg_f_function)
```

    ⎡t⋅v₁ + x₁⎤
    ⎢         ⎥
    ⎣t⋅v₂ + x₂⎦

```python
g = f.subs({"x1": new_arg_f_function[0],
            "x2": new_arg_f_function[1]})
```

```python
sympy.pprint(g)
```

               2              2
    (t⋅v₁ + x₁)  + (t⋅v₂ + x₂)

```{margin}

**Derivada de $g$ respecto a $t$: $Dg(t)=\nabla f(x+tv)^T v$.**

```

```python
sympy.pprint(g.diff(t))
```

    2⋅v₁⋅(t⋅v₁ + x₁) + 2⋅v₂⋅(t⋅v₂ + x₂)

**En lo siguiente se utiliza [derive-by_array](https://docs.sympy.org/latest/modules/tensor/array.html#derivatives-by-array), [how-to-get-the-gradient-and-hessian-sympy](https://stackoverflow.com/questions/39558515/how-to-get-the-gradient-and-hessian-sympy) para mostrar cómo se puede hacer un producto punto con SymPy**

```python
sympy.pprint(derive_by_array(f, x))
```

    ⎡2⋅x₁⎤
    ⎢    ⎥
    ⎣2⋅x₂⎦

```python
sympy.pprint(derive_by_array(f, x).subs({"x1": new_arg_f_function[0],
                                         "x2": new_arg_f_function[1]}))
```

    ⎡2⋅t⋅v₁ + 2⋅x₁⎤
    ⎢             ⎥
    ⎣2⋅t⋅v₂ + 2⋅x₂⎦

```python
gradient_f_new_arg = derive_by_array(f, x).subs({"x1": new_arg_f_function[0],
                                                 "x2": new_arg_f_function[1]})

```

```python
sympy.pprint(v)
```

    ⎡v₁⎤
    ⎢  ⎥
    ⎣v₂⎦

```{margin}

**Derivada de $g$ respecto a $t$: $Dg(t)=\nabla f(x+tv)^T v = v^T \nabla f(x + tv)$.**

```

```python
sympy.pprint(v.dot(gradient_f_new_arg))
```

    v₁⋅(2⋅t⋅v₁ + 2⋅x₁) + v₂⋅(2⋅t⋅v₂ + 2⋅x₂)

(EJ2)=

### Ejemplo

Si $h: \mathbb{R}^n \rightarrow \mathbb{R}$ dada por $h(x) = \log \left( \displaystyle \sum_{i=1}^m \exp(a_i^Tx+b_i) \right)$ con $x\in \mathbb{R}^n,a_i\in \mathbb{R}^n \forall i=1,\dots,m$ y $b_i \in \mathbb{R} \forall i=1,\dots,m$ entonces:

$$
Dh(x)=\left(\displaystyle \sum_{i=1}^m\exp(a_i^Tx+b_i) \right)^{-1}\left[ \begin{array}{c}
    \exp(a_1^Tx+b_1)\\
    \vdots\\
    \exp(a_m^Tx+b_m)
    \end{array}
    \right]^TA=(1^Tz)^{-1}z^TA
$$

donde: $A=(a_i)_{i=1}^m \in \mathbb{R}^{m\times n}, b \in \mathbb{R}^m$, $z=\left[ \begin{array}{c}
    \exp(a_1^Tx+b_1)\\
    \vdots\\
    \exp(a_m^Tx+b_m)
    \end{array}\right]$ y $1 \in \mathbb{R}^m$ es un vector con entradas iguales a $1$. Por lo tanto $\nabla h(x) = (1^Tz)^{-1}A^Tz$.

En este ejemplo $Dh(x) = Dg(f(x))Df(x)$ con:

* $h(x)=g(f(x))$,

* $g: \mathbb{R}^m \rightarrow \mathbb{R}$ dada por $g(y)=\log \left( \displaystyle \sum_{i=1}^m \exp(y_i) \right )$,

* $f(x)=Ax+b.$

Para lo siguiente se utilizó como referencias: [liga1](https://stackoverflow.com/questions/41581002/how-to-derive-with-respect-to-a-matrix-element-with-sympy), [liga2](https://docs.sympy.org/latest/modules/tensor/indexed.html), [liga3](https://stackoverflow.com/questions/37705571/sum-over-matrix-entries-in-sympy-1-0-with-python-3-5), [liga4](https://docs.sympy.org/latest/modules/tensor/array.html), [liga5](https://docs.sympy.org/latest/modules/concrete.html), [liga6](https://stackoverflow.com/questions/51723550/summation-over-a-sympy-array).

```python
m = sympy.Symbol('m')
n = sympy.Symbol('n')
```

```{margin}

Ver [indexed](https://docs.sympy.org/latest/modules/tensor/indexed.html)

```

```python
y = sympy.IndexedBase('y')
```

```python
i = sympy.Symbol('i') #for index of sum
```

```python
g = sympy.log(sympy.Sum(sympy.exp(y[i]), (i, 1, m)))
```

```{margin}

**Esta función es la que queremos derivar.**

```

```python
sympy.pprint(g)
```

       ⎛  m        ⎞
       ⎜ ___       ⎟
       ⎜ ╲         ⎟
       ⎜  ╲    y[i]⎟
    log⎜  ╱   ℯ    ⎟
       ⎜ ╱         ⎟
       ⎜ ‾‾‾       ⎟
       ⎝i = 1      ⎠

**Para un caso de $m=3$ en la función $g$ se tiene:**

```python
y1, y2, y3 = sympy.symbols("y1, y2, y3")
```

```python
g_m_3 = sympy.log(sympy.exp(y1) + sympy.exp(y2) + sympy.exp(y3))
```

```python
sympy.pprint(g_m_3)
```

       ⎛ y₁    y₂    y₃⎞
    log⎝ℯ   + ℯ   + ℯ  ⎠

```{margin}

Ver [derive-by_array](https://docs.sympy.org/latest/modules/tensor/array.html#derivatives-by-array)

```

```python
dg_m_3 = derive_by_array(g_m_3, [y1, y2, y3])
```

```{margin}

**Derivada de $g$ respecto a $y_1, y_2, y_3$.**

```

```python
sympy.pprint(dg_m_3)
```

    ⎡       y₁               y₂               y₃      ⎤
    ⎢      ℯ                ℯ                ℯ        ⎥
    ⎢───────────────  ───────────────  ───────────────⎥
    ⎢ y₁    y₂    y₃   y₁    y₂    y₃   y₁    y₂    y₃⎥
    ⎣ℯ   + ℯ   + ℯ    ℯ   + ℯ   + ℯ    ℯ   + ℯ   + ℯ  ⎦

```{margin}

Ver [Kronecker delta](https://en.wikipedia.org/wiki/Kronecker_delta)

```

```python
sympy.pprint(derive_by_array(g, [y[1], y[2], y[3]]))
```

    ⎡  m                 m                 m             ⎤
    ⎢ ____              ____              ____           ⎥
    ⎢ ╲                 ╲                 ╲              ⎥
    ⎢  ╲                 ╲                 ╲             ⎥
    ⎢   ╲   y[i]          ╲   y[i]          ╲   y[i]     ⎥
    ⎢   ╱  ℯ    ⋅δ        ╱  ℯ    ⋅δ        ╱  ℯ    ⋅δ   ⎥
    ⎢  ╱          1,i    ╱          2,i    ╱          3,i⎥
    ⎢ ╱                 ╱                 ╱              ⎥
    ⎢ ‾‾‾‾              ‾‾‾‾              ‾‾‾‾           ⎥
    ⎢i = 1             i = 1             i = 1           ⎥
    ⎢────────────────  ────────────────  ────────────────⎥
    ⎢    m                 m                 m           ⎥
    ⎢   ___               ___               ___          ⎥
    ⎢   ╲                 ╲                 ╲            ⎥
    ⎢    ╲    y[i]         ╲    y[i]         ╲    y[i]   ⎥
    ⎢    ╱   ℯ             ╱   ℯ             ╱   ℯ       ⎥
    ⎢   ╱                 ╱                 ╱            ⎥
    ⎢   ‾‾‾               ‾‾‾               ‾‾‾          ⎥
    ⎣  i = 1             i = 1             i = 1         ⎦

**Para la composición $h(x) = g(f(x))$ se utilizan las siguientes celdas:**

```{margin}

Ver [indexed](https://docs.sympy.org/latest/modules/tensor/indexed.html)
```

```python
A = sympy.IndexedBase('A')
x = sympy.IndexedBase('x')
```

```python
j = sympy.Symbol('j')
```

```python
b = sympy.IndexedBase('b')
```

```python
#we want something like:
sympy.pprint(sympy.exp(sympy.Sum(A[i, j]*x[j], (j, 1, n)) + b[i]))
```

              n
             ___
             ╲
              ╲
     b[i] +   ╱   A[i, j]⋅x[j]
             ╱
             ‾‾‾
            j = 1
    ℯ

```python
#better if we split each step:
arg_sum = A[i, j]*x[j]
```

```python
sympy.pprint(arg_sum)
```

    A[i, j]⋅x[j]

```python
arg_exp = sympy.Sum(arg_sum, (j, 1, n)) + b[i]
```

```python
sympy.pprint(arg_exp)
```

             n
            ___
            ╲
             ╲
    b[i] +   ╱   A[i, j]⋅x[j]
            ╱
            ‾‾‾
           j = 1

```python
sympy.pprint(sympy.exp(arg_exp))
```

              n
             ___
             ╲
              ╲
     b[i] +   ╱   A[i, j]⋅x[j]
             ╱
             ‾‾‾
            j = 1
    ℯ

```python
arg_2_sum = sympy.exp(arg_exp)
```

```python
sympy.pprint(sympy.Sum(arg_2_sum, (i, 1, m)))
```

       m
    _______
    ╲
     ╲
      ╲               n
       ╲             ___
        ╲            ╲
         ╲            ╲
         ╱   b[i] +   ╱   A[i, j]⋅x[j]
        ╱            ╱
       ╱             ‾‾‾
      ╱             j = 1
     ╱      ℯ
    ╱
    ‾‾‾‾‾‾‾
     i = 1

```python
h = sympy.log(sympy.Sum(arg_2_sum, (i, 1, m)))
#complex expression: sympy.log(sympy.Sum(sympy.exp(sympy.Sum(A[i, j]*x[j], (j, 1, n)) + b[i]), (i, 1, m)))
```

```python
sympy.pprint(h)
```

       ⎛   m                              ⎞
       ⎜_______                           ⎟
       ⎜╲                                 ⎟
       ⎜ ╲                                ⎟
       ⎜  ╲               n               ⎟
       ⎜   ╲             ___              ⎟
       ⎜    ╲            ╲                ⎟
       ⎜     ╲            ╲               ⎟
    log⎜     ╱   b[i] +   ╱   A[i, j]⋅x[j]⎟
       ⎜    ╱            ╱                ⎟
       ⎜   ╱             ‾‾‾              ⎟
       ⎜  ╱             j = 1             ⎟
       ⎜ ╱      ℯ                         ⎟
       ⎜╱                                 ⎟
       ⎜‾‾‾‾‾‾‾                           ⎟
       ⎝ i = 1                            ⎠

```{margin}

**Derivada de $h$ respecto a $x_1$.**
```

```python
sympy.pprint(h.diff(x[1]))
```

       m
    ________
    ╲
     ╲                 n
      ╲               ___
       ╲              ╲
        ╲              ╲
         ╲    b[i] +   ╱   A[i, j]⋅x[j]   n
          ╲           ╱                  ___
          ╱           ‾‾‾                ╲
         ╱           j = 1                ╲   δ   ⋅A[i, j]
        ╱    ℯ                         ⋅  ╱    1,j
       ╱                                 ╱
      ╱                                  ‾‾‾
     ╱                                  j = 1
    ╱
    ‾‾‾‾‾‾‾‾
     i = 1
    ──────────────────────────────────────────────────────
                 m
              _______
              ╲
               ╲
                ╲               n
                 ╲             ___
                  ╲            ╲
                   ╲            ╲
                   ╱   b[i] +   ╱   A[i, j]⋅x[j]
                  ╱            ╱
                 ╱             ‾‾‾
                ╱             j = 1
               ╱      ℯ
              ╱
              ‾‾‾‾‾‾‾
               i = 1

```{margin}

Ver [Kronecker delta](https://en.wikipedia.org/wiki/Kronecker_delta)
```

```python
sympy.pprint(derive_by_array(h, [x[1]])) #we can use also: derive_by_array(h, [x[1], x[2], x[3]]
```

    ⎡   m                                                  ⎤
    ⎢________                                              ⎥
    ⎢╲                                                     ⎥
    ⎢ ╲                 n                                  ⎥
    ⎢  ╲               ___                                 ⎥
    ⎢   ╲              ╲                                   ⎥
    ⎢    ╲              ╲                                  ⎥
    ⎢     ╲    b[i] +   ╱   A[i, j]⋅x[j]   n               ⎥
    ⎢      ╲           ╱                  ___              ⎥
    ⎢      ╱           ‾‾‾                ╲                ⎥
    ⎢     ╱           j = 1                ╲   δ   ⋅A[i, j]⎥
    ⎢    ╱    ℯ                         ⋅  ╱    1,j        ⎥
    ⎢   ╱                                 ╱                ⎥
    ⎢  ╱                                  ‾‾‾              ⎥
    ⎢ ╱                                  j = 1             ⎥
    ⎢╱                                                     ⎥
    ⎢‾‾‾‾‾‾‾‾                                              ⎥
    ⎢ i = 1                                                ⎥
    ⎢──────────────────────────────────────────────────────⎥
    ⎢             m                                        ⎥
    ⎢          _______                                     ⎥
    ⎢          ╲                                           ⎥
    ⎢           ╲                                          ⎥
    ⎢            ╲               n                         ⎥
    ⎢             ╲             ___                        ⎥
    ⎢              ╲            ╲                          ⎥
    ⎢               ╲            ╲                         ⎥
    ⎢               ╱   b[i] +   ╱   A[i, j]⋅x[j]          ⎥
    ⎢              ╱            ╱                          ⎥
    ⎢             ╱             ‾‾‾                        ⎥
    ⎢            ╱             j = 1                       ⎥
    ⎢           ╱      ℯ                                   ⎥
    ⎢          ╱                                           ⎥
    ⎢          ‾‾‾‾‾‾‾                                     ⎥
    ⎣           i = 1                                      ⎦

```{admonition} Pregunta
:class: tip

¿Se puede resolver este ejercicio con [Matrix Symbol](https://docs.sympy.org/latest/modules/matrices/expressions.html)?
```

```{admonition} Ejercicio
:class: tip

Verificar que lo obtenido con SymPy es igual a lo desarrollado en "papel" al inicio del {ref}`Ejemplo <EJ2>`
```

## Segunda derivada de una función $f: \mathbb{R}^n \rightarrow \mathbb{R}$.

```{admonition} Definición

Sea $f:\mathbb{R}^n \rightarrow \mathbb{R}$. La segunda derivada o matriz **Hessiana** de $f$ en $x \in \text{intdom}f$ existe si $f$ es dos veces diferenciable en $x$, se denota $\nabla^2f(x)$ y sus componentes son segundas derivadas parciales:

$$\nabla^2f(x) = \left[\begin{array}{cccc}
\frac{\partial^2f(x)}{\partial x_1^2} &\frac{\partial^2f(x)}{\partial x_2 \partial x_1}&\dots&\frac{\partial^2f(x)}{\partial x_n \partial x_1}\\
\frac{\partial^2f(x)}{\partial x_1 \partial x_2} &\frac{\partial^2f(x)}{\partial x_2^2} &\dots&\frac{\partial^2f(x)}{\partial x_n \partial x_2}\\
\vdots &\vdots& \ddots&\vdots\\
\frac{\partial^2f(x)}{\partial x_1 \partial x_n} &\frac{\partial^2f(x)}{\partial x_2 \partial x_n}&\dots&\frac{\partial^2f(x)}{\partial x_n^2} \\
\end{array}
\right]
$$
```

```{admonition} Comentarios:

* La aproximación de segundo orden a $f$ en $x$ (o también para puntos cercanos a $x$) es la función cuadrática en la variable $z$:

$$f(x) + \nabla f(x)^T(z-x)+\frac{1}{2}(z-x)^T\nabla^2f(x)(z-x)$$

* Se cumple:

$$\displaystyle \lim_{z \rightarrow x, z \neq x} \frac{|f(z)-[f(x)+\nabla f(x)^T(z-x)+\frac{1}{2}(z-x)^T\nabla^2f(x)(z-x)]|}{||z-x||_2} = 0, z \in \text{dom}f$$

* Se tiene lo siguiente:

    * $\nabla f$ es una función nombrada *gradient mapping* (o simplemente gradiente).

    * $\nabla f:\mathbb{R}^n \rightarrow \mathbb{R}^n$ tiene regla de correspondencia $\nabla f(x)$ (evaluar en $x$ la matriz $Df(\cdot)^T$).

    * Se dice que $f$ es dos veces diferenciable en $\text{dom}f$ si $\text{dom}f$ es abierto y $f$ es dos veces diferenciable en cada punto de $x$.

    * $D\nabla f(x) = \nabla^2f(x)$ para $x \in \text{intdom}f$.

    * $\nabla ^2 f(x) : \mathbb{R}^n \rightarrow \mathbb{R}^{n \times n}$.

    * Si $f \in \mathcal{C}^2(\text{dom}f)$ entonces la Hessiana es una matriz simétrica.

```

## Regla de la cadena para la segunda derivada

(CP2)=

### Caso particular

Sean:

* $f:\mathbb{R}^n \rightarrow \mathbb{R}$,

* $g:\mathbb{R} \rightarrow \mathbb{R}$,

* $h:\mathbb{R}^n \rightarrow \mathbb{R}$ con  $h(x) = g(f(x))$, entonces:

$$\nabla^2h(x) = D\nabla h(x)$$

```{margin}

Ver {ref}`Ejemplo 1 de la regla de la cadena <EJ1>`

```

y

$$\nabla h(x)=Dh(x)^T = (Dg(f(x))Df(x))^T=\frac{dg(f(x))}{dx}\nabla f(x)$$

por lo que:

$$
\begin{eqnarray}
\nabla^2 h(x) &=& D\nabla h(x) \nonumber \\
&=& D \left(\frac{dg(f(x))}{dx}\nabla f(x)\right) \nonumber \\
&=& \frac{dg(f(x))}{dx}\nabla^2 f(x)+\left(\frac{d^2g(f(x))}{dx}\nabla f(x) \nabla f(x)^T \right)^T \nonumber \\
&=& \frac{dg(f(x))}{dx}\nabla^2 f(x)+\frac{d^2g(f(x))}{dx} \nabla f(x) \nabla f(x)^T \nonumber
\end{eqnarray}
$$

(CP3)=

### Caso particular

Sean:

* $f:\mathbb{R}^n \rightarrow \mathbb{R}^m, f(x) = Ax+b$ con $A \in \mathbb{R}^{m\times n}$, $b \in \mathbb{R}^m$,

* $g:\mathbb{R}^m \rightarrow \mathbb{R}^p$,

* $h:\mathbb{R}^n \rightarrow \mathbb{R}^p$, $h(x) = g(f(x)) = g(Ax+b)$ con $\text{dom}h=\{z \in \mathbb{R}^n | Az+b \in \text{dom}g\}$ entonces:

```{margin}

Ver {ref}`Caso particular <CP1>` para la expresión de la derivada.
```

$$Dh(x)^T = Dg(f(x))Df(x) = Dg(Ax+b)A.$$

```{admonition} Observación
:class: tip

Si $p=1$, $g: \mathbb{R}^m \rightarrow \mathbb{R}$, $h: \mathbb{R}^n \rightarrow \mathbb{R}$ se tiene:

$$\nabla^2h(x) = D \nabla h(x) = A^T \nabla^2g(Ax+b)A.$$

```

### Ejemplo

```{margin}

Ver {ref}`Ejemplo <EJRestriccionALinea>`

```

Si $f:\mathbb{R}^n \rightarrow \mathbb{R}$, $g: \mathbb{R} \rightarrow \mathbb{R}$ está dada por $g(t) = f(x+tv)$ con $x,v \in \mathbb{R}^n, t \in \mathbb{R}$, esto es, $g$ es $f$ pero restringida a la línea $\{x+tv|t \in \mathbb{R}\}$ , entonces:

$$Dg(t) = Df(x+tv)v = \nabla f(x+tv)^Tv$$

Por lo que:

$$\nabla ^2g(t) = D\nabla f(x+tv)^Tv=v^T\nabla^2f(x+tv)v.$$

### Ejemplo

```{margin}

Ver {ref}`Ejemplo <EJ2>`

```

Si $h: \mathbb{R}^n \rightarrow \mathbb{R}, h(x) = \log \left( \displaystyle \sum_{i=1}^m \exp(a_i^Tx+b_i)\right)$ con $x \in \mathbb{R}^n, a_i \in \mathbb{R}^n \forall i=1,\dots,m$ y $b_i \in \mathbb{R} \forall i=1,\dots,m$.

Como se desarrolló anteriormente $\nabla h(x) = (1^Tz)^{-1}A^Tz$ con $z=\left[ \begin{array}{c}
    \exp(a_1^Tx+b_1)\\
    \vdots\\
    \exp(a_m^Tx+b_m)
    \end{array}\right]$ y $A=(a_i)_{i=1}^m \in \mathbb{R}^{m\times n}.$

Por lo que

$$\nabla^2 h(x) = D\nabla h(x) = A^T \nabla^2g(Ax+b)A$$

```{margin}

$\nabla^2 g(y)$ se obtiene de acuerdo a {ref}`Caso particular <CP2>` tomando $\log:\mathbb{R} \rightarrow \mathbb{R}, \displaystyle \sum_{i=1}^m \exp(y_i): \mathbb{R}^m \rightarrow \mathbb{R}$

```

donde: $\nabla^2g(y)=(1^Ty)^{-1}\text{diag}(y)-(1^Ty)^{-2}yy^T$.

$$\therefore  \nabla^2 h(x) = A^T\left[(1^Tz)^{-1}\text{diag}(z)-(1^Tz)^{-2}zz^T \right]A$$

y $\text{diag}(c)$ es una matriz diagonal con elementos en su diagonal iguales a las entradas del vector $c$.

```{admonition} Ejercicio
:class: tip

Verificar con el paquete de SymPy las expresiones para la segunda derivada de los dos ejemplos anteriores.

```

## Tablita útil para fórmulas de diferenciación con el operador $\nabla$

Sean $f,g:\mathbb{R}^n \rightarrow \mathbb{R}$ con $f,g \in \mathcal{C}^2$ respectivamente en sus dominios y $\alpha_1, \alpha_2 \in \mathbb{R}$, $A \in \mathbb{R}^{n \times n}$, $b \in \mathbb{R}^n$ son fijas. Diferenciando con respecto a la variable $x \in \mathbb{R}^n$ se tiene:

| | |
|:--:|:--:|
|linealidad | $\nabla(\alpha_1 f(x) + \alpha_2 g(x)) = \alpha_1 \nabla f(x) + \alpha_2 \nabla g(x)$|
|producto | $\nabla(f(x)g(x)) = \nabla f(x) g(x) + f(x) \nabla g(x)$|
|producto punto|$\nabla(b^Tx) = b$
|cuadrático|$\nabla(x^TAx) = 2(A+A^T)x$|
|segunda derivada| $\nabla^2(Ax)=A$|

## Comentario respecto al cómputo simbólico o algebraico y númerico

Si bien el cómputo simbólico o algebraico nos ayuda a calcular las expresiones para las derivadas evitando los problemas de errores por redondeo que se revisarán en {ref}`Polinomios de Taylor y diferenciación numérica <PTDN>`, la complejidad de las expresiones que internamente se manejan es ineficiente vs el cómputo numérico, ver [Computer science aspects of computer algebra](https://en.wikipedia.org/wiki/Computer_algebra#Computer_science_aspects) y [GNU_Multiple_Precision_Arithmetic_Library](https://en.wikipedia.org/wiki/GNU_Multiple_Precision_Arithmetic_Library).

Como ejemplo de la precisión arbitraria que se puede manejar con el cómputo simbólico o algebraico vs el {ref}`Sistema en punto flotante <SPF>` considérese el cálculo siguiente:

```python
eps = 1-3*(4/3-1)
```

```python
print("{:0.16e}".format(eps))
```

    '2.2204460492503131e-16'

```python
eps_sympy = 1-3*(sympy.Rational(4,3)-1)
```

```python
print("{:0.16e}".format(float(eps_sympy)))
```

    '0.0000000000000000e+00'

```{admonition} Ejercicios
:class: tip

1.Resuelve los ejercicios y preguntas de la nota.
```

**Referencias**

1. S. P. Boyd, L. Vandenberghe, Convex Optimization. Cambridge University Press, 2004.


```
from scipy.integrate import ode
from models import nmda, LR, LR_ICa, HH, na_current, cable
#from sympy import plot_implicit, symbols, Eq
#import sympy
%pylab inline
%load_ext autoreload
%autoreload 2
```

    Populating the interactive namespace from numpy and matplotlib

```
def nmda_impulse(t, y, T=10, t_in=500, t_end=1500):
    rb = 0.005
    dy = nmda(y)
    if t_in <= t < t_end:
        dy[0] += -rb * T * y[0]
        dy[1] += rb * T * y[0] - rb * y[1] * T
        dy[2] += rb * y[1]
    return dy

def ica(O, V):
    Mg = 2.0
    gNMDA = 1.0
    VCa = 100.0
    B = 1.0 / (1 + exp(-0.062 * V)*Mg/3.57)
    ICa = gNMDA * B * O * (V - VCa)
    return ICa

def v_dendrite(t, y):
    dy = zeros_like(y)
    dy[:4] = nmda_impulse(t, y[:4])
    ICa = ica(y[3], y[4])
    dy[4] = -ICa - 0.3*y[4]
    return dy

def nmda_LR(t, y):
    dy = zeros_like(y)
    dy[:4] = nmda_impulse(t, y[:4])
    ICa = ica(y[3], y[4])
    dy[4:] = LR(y[4:], ICa)
    #dy[6] = 0.007*(-0.0275* ICa - 0.02*y[6])
    return dy

def nmda_HH(t, y):
    dy = zeros_like(y)
    dy[:4] = HH(y[:4])
    dy[4:] = nmda_impulse(t, y[4:])
    Ica = ca_current(y[7], y[0])
    dy[0] -= Ica
    return dy

solver = ['dopri5', 'dop853', 'vode']
```

# Hodgkin and Huxley

```
x, y = symbols('x y')
gna = 120.0
gk = 36.0
gl = 0.3
Vna = 115.0
Vk = -12.0
Vl = 10.6
alpha_m = 0.1 * (25.0 - x) / (sympy.exp((25.0 - x) / 10.0) - 1.0)
beta_m = 4.0 * sympy.exp(-x / 18.0)
m_inf = alpha_m / (alpha_m + beta_m)
INa = gna * m_inf**3 * (0.8 - y) * (x - Vna)
IK = gk * y**4 * (x - Vk)
IL = gl * (x - Vl)

plot_implicit(Eq(INa + IK + IL, 0), (x, -20, 120), (y, 0, 1), adaptive=False);

```

```
#r = ode(  lambda t, y: HH(y) + array([5, 0, 0, 0]) if 5<= t < 7 else HH(y)).set_integrator(solver[0])
HH_input = lambda t, y: HH(y) + array([140, 0, 0, 0]) if 100<= t < 1100 else HH(y)
#print(HH_input(array(1, 0, 0, 0),0))
y0 = array([0.0, 0.3176, 0.053, 0.596])
r = ode(HH_input).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 1200.0
dt = .01
v = []
time = []
while r.successful() and r.t < t1:
    r.integrate(r.t+dt)
    v.append(r.y[0])
    time.append(r.t)
figure(1)
plot(time, v)
xlabel('timepo (ms)')
ylabel('V (mV)')
grid(True)

```

#Rinzel and Lee
$C_m \frac{dv}{dt}=-I_{Ca}   −\left(g_K n^4 +\frac{g_{K, Ca} c}{K_d + c}\right) (V − V K ) − g_L (V − V_L )$

$\tau_n \frac{dn}{dt}=n_{\inf}(V)-n$

$\frac{dc}{dt} = f(-k_1 I_{Ca} -k_c c)$

```
#y0 = array([-50.0, 0.530, 0.7])
#r = ode(lambda t, y: LR(y)).set_integrator(solver[0])
y0 = array([-50.0, 0.530, 0.1])
r = ode(lambda t, y: array([LR(y)[0], LR(y)[1], 0])).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 1000.0
dt = 1.0
v = []
n = []
ca = []
time = []
while r.successful() and r.t < t1:
    r.integrate(r.t+dt)
    v.append(r.y[0])
    n.append(r.y[1])
    ca.append(r.y[2])
    time.append(r.t)
figure(1)
plot(time, v)
xlabel('timepo (ms)')
ylabel('V (mV)')
grid(True)

figure(2)
plot(time, n)
xlabel('timepo (ms)')
ylabel('n')
grid(True)

figure(3)
plot(time, ca)
xlabel('timepo (ms)')
ylabel('[Ca] (M)')
grid(True)

show()
```

#NMDA Channel

```
y0 = array([1, 0, 0, 0, 0])
#r = ode(lambda t, y: nmda_impulse(t, y)).set_integrator(solver[0])
r = ode(lambda t, y: v_dendrite(t, y)).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 4000.0
dt = 1.0
o = []
ICa = []
v = []
time = []
while r.successful() and r.t < t1:
    r.integrate(r.t + dt)
    o.append(r.y[3])
    v.append(r.y[4])
    time.append(r.t)
figure(1)
plot(time, o)
grid(True)
xlabel('timepo (ms)')
ylabel('Fraccion de canales abiertos')

figure(2)
plot(time, v)
grid(True)
xlabel('timepo (ms)')
ylabel('V')

show()
```

```
y0 = array([1, 0, 0, 0, -50.0, 0.530, 0.4])
r = ode(nmda_LR).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 4000.0
dt = 1.0
v = []
ca = []
time = []
while r.successful() and r.t < t1:
    r.integrate(r.t+dt)
    v.append(r.y[4])
    ca.append(r.y[6])
    time.append(r.t)
figure(1)
plot(time, v)
xlabel('timepo (s)')
ylabel('V (mV)')
grid(True)
figure(2)
plot(time, ca)
grid(True)

show()
```

```
y0 = array([0.0, 0.3176, 0.053, 0.596])
r = ode(lambda t, y: HH(y) + array([5, 0, 0, 0]) if 5<= t < 7 else HH(y)).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 20
dt = 0.01
v = []
time = []
while r.successful() and r.t < t1:
    r.integrate(r.t+dt)
    v.append(r.y[0])
    time.append(r.t)
figure(1)
plot(time, v)
xlabel('timepo (ms)')
ylabel('V (mV)')
grid(True)
```

```
y0 = array([-40.0, 0.3176, 0.053, 0.596, 1, 0, 0, 0])
r = ode(nmda_HH).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 2000.0
dt = 1.0
v = []
o = []
ICa = []
time = []
while r.successful() and r.t < t1: #
    r.integrate(r.t+dt)
    v.append(r.y[0])
    o.append(r.y[7])
    ICa.append(ica(r.y[7], r.y[0]))
    time.append(r.t)
figure(1)
plot(time, v)
xlabel('timepo (ms)')
ylabel('V (mV)')
grid(True)

figure(2)
plot(time, o)
xlabel('timepo (ms)')
ylabel('fraction open')
grid(True)

figure(3)
plot(time, ICa)
xlabel('timepo (ms)')
ylabel('Calcium Current')
grid(True)
```

#Cable

```
y0 = zeros((100,))
y0[40:50] = 10 * ones(10,)
r = ode(lambda t, y: cable(y)).set_integrator(solver[0])
r.set_initial_value(y0, 0)
t1 = 10.0
dt = 1.0
v = [y0]
time = []
while r.successful() and r.t < t1: #
    r.integrate(r.t+dt)
    v.append(r.y)
    time.append(r.t)
figure(1)
for x in v:
    plot(x)
xlim([30, 50])
ylim([0, 0.01])
xlabel('distancia')
ylabel('V (mV)')
grid(True)

```

```
a = linspace(0, 9, 10)
N = a.shape[0]
print(a[1:N-1].shape)
print(a[:N-2].shape)
print(a[2:].shape)
```

    (8,)
    (8,)
    (8,)


```python
import os, sys
import h5py
import numpy as np
from scipy.io import loadmat
import cv2
import matplotlib
%matplotlib inline
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from numpy import matrix as mat
from sympy import *
from numpy import linalg as la
```

```python
def getFx(para, frame):   # para中为(13*frame+3)的一套参数，frame传进来是为了确定循环次数
    #先写出参数表达式，ABDCDEF六个点的齐次坐标
    K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                     [0.0, 1147.59161666764, 508.064917088557],
                     [0.0, 0.0, 1.0]])
    r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
    Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
    a, b, c, th, al = symbols('a b c th al')
    ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
    f = Symbol('f')
    XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
    XB = Matrix([[0], [c], [0], [1]])
    XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
    XD = Matrix([[-b * c], [0], [0], [1]])
    XE = Matrix([[0], [0], [0], [1]])
    XF = Matrix([[b * c], [0], [0], [1]])
    ua, va, wa = K[0,:] * (Rt * XA), K[1,:] * (Rt * XA), K[2,:] * (Rt * XA)
    ub, vb, wb = K[0,:] * (Rt * XB), K[1,:] * (Rt * XB), K[2,:] * (Rt * XB)
    uc, vc, wc = K[0,:] * (Rt * XC), K[1,:] * (Rt * XC), K[2,:] * (Rt * XC)
    ud, vd, wd = K[0,:] * (Rt * XD), K[1,:] * (Rt * XD), K[2,:] * (Rt * XD)
    ue, ve, we = K[0,:] * (Rt * XE), K[1,:] * (Rt * XE), K[2,:] * (Rt * XE)
    uf, vf, wf = K[0,:] * (Rt * XF), K[1,:] * (Rt * XF), K[2,:] * (Rt * XF)
    #根据每一帧的循环，提取出Rt的参数，K是公用的，代入参数写出3D坐标，并计算出u/w，v/w
    #写成f的形式，即按照六个点分块，每块里面有M帧
    getfx = mat(np.zeros((6*frame*2,1)))
    for i in range(6):
        for j in range(frame):
            if i == 0 :
                f = Matrix([ua/wa, va/wa])
            elif i == 1 :
                f = Matrix([ub/wb, vb/wb])
            elif i == 2 :
                f = Matrix([uc/wc, vc/wc])
            elif i == 3 :
                f = Matrix([ud/wd, vd/wd])
            elif i == 4 :
                f = Matrix([ue/we, ve/we])
            else:
                f = Matrix([uf/wf, vf/wf])
            f_value = f.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
                        r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
                        r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
                        al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
            getfx[i*frame*2+j*2] = f_value[0]
            getfx[i*frame*2+j*2+1] = f_value[1]
    #返回getfx值，2*frame*6 by 1
    return getfx
```

```python
def getJacobian(point, frame, para):
    # 用参数表示K，R矩阵
    focalx, focaly, px, py = symbols('focalx focaly px py')
    r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
    Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
    K = Matrix([[focalx, 0, px], [0, focaly, py], [0, 0, 1]])
    # KRt = K * Rt
    # 用参数表示ABCDEF六个点坐标
    a, b, c, th, al = symbols('a b c th al')
    ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
    f = Symbol('f')
    if point == 0 :
        XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
        ua, va, wa = K[0,:] * (Rt * XA), K[1,:] * (Rt * XA), K[2,:] * (Rt * XA)
        f = Matrix([ua/wa, va/wa])
    elif point == 1 :
        XB = Matrix([[0], [c], [0], [1]])
        ub, vb, wb = K[0,:] * (Rt * XB), K[1,:] * (Rt * XB), K[2,:] * (Rt * XB)
        f = Matrix([ub/wb, vb/wb])
    elif point == 2 :
        XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
        uc, vc, wc = K[0,:] * (Rt * XC), K[1,:] * (Rt * XC), K[2,:] * (Rt * XC)
        f = Matrix([uc/wc, vc/wc])
    elif point == 3 :
        XD = Matrix([[-b * c], [0], [0], [1]])
        ud, vd, wd = K[0,:] * (Rt * XD), K[1,:] * (Rt * XD), K[2,:] * (Rt * XD)
        f = Matrix([ud/wd, vd/wd])
    elif point == 4 :
        XE = Matrix([[0], [0], [0], [1]])
        ue, ve, we = K[0,:] * (Rt * XE), K[1,:] * (Rt * XE), K[2,:] * (Rt * XE)
        f = Matrix([ue/we, ve/we])
    elif point == 5:
        XF = Matrix([[b * c], [0], [0], [1]])
        uf, vf, wf = K[0,:] * (Rt * XF), K[1,:] * (Rt * XF), K[2,:] * (Rt * XF)
        f = Matrix([uf/wf, vf/wf])
    args = Matrix([r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33, th, al, a, b, c])
    f_X1 = f[0,:].jacobian(args)
    f_X2 = f[1,:].jacobian(args)
    JA = Matrix([f_X1, f_X2])   # 2 by 16 matrix
    JA_value = JA.subs({focalx:1149.676, focaly:1147.592, px:508.849, py:508.065, r11:para[13*frame], r12:para[13*frame+1],
                                 r13:para[13*frame+2], r14:para[13*frame+3], r21:para[13*frame+4], r22:para[13*frame+5],
                                 r23:para[13*frame+6], r24:para[13*frame+7], r31:para[13*frame+8], r32:para[13*frame+9],
                                 r33:para[13*frame+10], th:para[13*frame+11], al:para[13*frame+12], a:para[-3], b:para[-2], c:para[-1]})
    #JA_value = JA_value.subs({f:1149.68})
    return JA_value
```

```python
def getJ(para, frame):
    getj = mat(np.zeros((6*frame*2, 13*frame+3)))
    for m in range(6):
        for n in range(frame):
            JA_value = getJacobian(m, n, para)
            #print(JA_value)
            getj[2*(m*frame+n):2*(m*frame+n+1), 13*n:13*n+13] = JA_value[:, 0:13]
            getj[2*(m*frame+n):2*(m*frame+n+1), -3:] = JA_value[:, -3:]
    return getj
```

```python
def getE(getfx, frame):
    E = mat(np.zeros((6*frame*2,1)))
    for i in range(6):
        for j in range(frame):
            if i==0 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==1 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==2 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==3 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==4 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==5 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
    return E
```

```python
def LM_opti(frame, x_para, u=1, v=2, step_max=500):
    J = mat(np.zeros((6*frame*2, 13*frame+3)))
    E = mat(np.zeros((6*frame*2,1)))   # E = f(X) - b ;
    E_temp = mat(np.zeros((6*frame*2,1)))   # E_temp compare with E in L-M
    x_k = mat(x_para.copy())   #parameter initialization
    step = 0   # iteration steps
    mse_last = 0   # mse value after iteration each time
    step_max = 500   # maximum number of iteration
    u = 1
    v = 2   # u, v initial value
    # L-M Algorithm obtain optimal parameters
    while(step < step_max):
        step += 1
        mse, mse_temp = 0, 0

        # generate Jacobian Matrix and calculate E
        getfx = mat(np.zeros((6*frame*2,1)))
        getfx = getFx(x_k, frame)
        E = getE(getfx, frame)

        for i in range(6*frame*2):
            mse += E[i]**2
        mse /= 6*frame*2

        # get new J
        J = mat(np.zeros((6*frame*2, 13*frame+3)))
        J = getJ(x_k, frame)

        # delta X = ...
        #print(J.T * J)
        dx = mat(np.zeros((13*frame+3,1)))
        LM = u * mat(np.eye(13*frame+3))
        dx = -(J.T * J + LM).I * J.T * E
        x_k_temp = x_k.copy()
        x_k_temp += dx

        #R的更新不能简单赋值
        #get R meet R.T*R=I
        #U * D * V.T = R --> R+ = U * V.T
        R_old = mat([[x_k_temp[0,0], x_k_temp[1,0], x_k_temp[2,0]],
                     [x_k_temp[4,0], x_k_temp[5,0], x_k_temp[6,0]],
                     [x_k_temp[8,0], x_k_temp[9,0], x_k_temp[10,0]]])
        U, sigma, VT = la.svd(R_old)
        R_new = U * VT
        x_k_temp[0,0], x_k_temp[1,0], x_k_temp[2,0] = R_new[0,0], R_new[0,1], R_new[0,2]
        x_k_temp[4,0], x_k_temp[5,0], x_k_temp[6,0] = R_new[1,0], R_new[1,1], R_new[1,2]
        x_k_temp[8,0], x_k_temp[9,0], x_k_temp[10,0] = R_new[2,0], R_new[2,1], R_new[2,2]
        ###########

        # calculate E_temp with x_k_temp
        # copy from E with x_k
        getfx_temp = mat(np.zeros((6*frame*2,1)))
        getfx_temp = getFx(x_k_temp, frame)
        E_temp = getE(getfx_temp, frame)

        for i in range(6*frame*2):
            mse_temp += E_temp[i]**2
        mse_temp /= 6*frame*2

        # segma value to choose optimization model
        segma = (mse - mse_temp)/((dx.T * (u * dx - J.T * E))[0,0])

        # calculate new u
        if segma > 0:
            s = 1.0/3.0
            v = 2
            x_k = x_k_temp
            mse = mse_temp
            u = u * max(s, 1-pow(2*segma,3))
            u = u[0,0]
        else:
            u = u * v
            v = v * 2
            x_k = x_k_temp

        print("step = %d, abs(mse-mse_last) = %.8f" %(step, abs(mse-mse_last)))
        if abs(mse-mse_last)<0.000001:
            break
        mse_last = mse

    print("step = ", step)
    print("mse = ", mse_last)
    #print("parameter = ", x_k)

    return x_k
```

```python
# 数据读取
frame = 1
m = loadmat("valid.mat")
# camera intrinsic matrix
K = m["annot"][0][0][4]
K_cam = K[0][0].tolist()
# key point 3D groundtruth
gt = m["annot"][0][0][3]
img1_gt = gt[135]   # array 3 by 17
kp = np.zeros((17,2))
for i in range(17):
    u = K_cam[0] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    v = K_cam[1] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    w = K_cam[2] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    kp[i][0] = u/w
    kp[i][1] = v/w
# load  and show image
img = cv2.imread("S9_Posing_1.55011271_000676.jpg")
plt.figure("Image") # 图像窗口名称
plt.imshow(img[:,:,[2,1,0]])
plt.axis('on') # 关掉坐标轴为 off
plt.title('image1') # 图像题目
plt.show()
# visualize key points
txt = ['1','2','3','4','5','6','7','8','9','10','11','12','13','14','15','16','17']
img_kp = plt.scatter(kp[:,0], kp[:,1], s = 80, c = 'g', marker = 'X')
for i in range(17):
    plt.annotate(txt[i], xy = (kp[i,0], kp[i,1]), xytext = (kp[i,0]+0.1, kp[i,1]+0.1)) # 这里xy是需要标记的坐标，xytext是对应的标签坐标
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp') # 图像题目
# visualize ABCDEF
plt.figure()
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF') # 图像题目
plt.show()
# save 2D coordinate to list
x2d = np.zeros((6 * frame,2))
x2d[0,0] = kp[0,0]
for i in range(6):
    for j in range(frame):
        if i==0 :
            x2d[i*frame+j, 0] = kp[14, 0]
            x2d[i*frame+j, 1] = kp[14, 1]
        elif i==1 :
            x2d[i*frame+j, 0] = kp[8, 0]
            x2d[i*frame+j, 1] = kp[8, 1]
        elif i==2 :
            x2d[i*frame+j, 0] = kp[11, 0]
            x2d[i*frame+j, 1] = kp[11, 1]
        elif i==3 :
            x2d[i*frame+j, 0] = kp[4, 0]
            x2d[i*frame+j, 1] = kp[4, 1]
        elif i==4 :
            x2d[i*frame+j, 0] = kp[0, 0]
            x2d[i*frame+j, 1] = kp[0, 1]
        elif i==5 :
            x2d[i*frame+j, 0] = kp[1, 0]
            x2d[i*frame+j, 1] = kp[1, 1]
print(x2d)
```

```python
# parameter initialization for all frame (K_cam, x2d(6*frame by 2))
# x_para(13*frame+3)
x_para = np.zeros((13*frame+3,1))
for i in range(frame):
    x_para[13*i] = -1   # r11
    x_para[13*i+5] = 1   # r22
    x_para[13*i+10] = -1   # r33
    x_para[13*i+3] = 0.0047
    x_para[13*i+7] = -0.0997
    x_para[13*i+11] = 0   # th
    x_para[13*i+12] = 0   #al
x_para[-3] = 0.35   # a
x_para[-2] = 0.25   # b
distance = -0.095#0.096   # c
x_para[-1] = distance
print(mat(x_para.copy()))
getfx_ini = getFx(x_para, frame)
print(getfx_ini)
```

    [[-1.    ]
     [ 0.    ]
     [ 0.    ]
     [ 0.0047]
     [ 0.    ]
     [ 1.    ]
     [ 0.    ]
     [-0.0997]
     [ 0.    ]
     [ 0.    ]
     [-1.    ]
     [ 0.    ]
     [ 0.    ]
     [ 0.35  ]
     [ 0.25  ]
     [-0.095 ]]
    [[476.02538041]
     [284.62882932]
     [514.25209744]
     [284.62882932]
     [552.47881446]
     [284.62882932]
     [486.94729956]
     [393.65003291]
     [514.25209744]
     [393.65003291]
     [541.55689531]
     [393.65003291]]

```python
plt.figure()
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[0,0], getfx_ini[1,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[2,0], getfx_ini[3,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[4,0], getfx_ini[5,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[6,0], getfx_ini[7,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[8,0], getfx_ini[9,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[10,0], getfx_ini[11,0], s = 80, c = 'r', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF_original_opt') # 图像题目
plt.show()
```

```python
x_k = LM_opti(frame, x_para)
R_old = mat([[x_k[0,0], x_k[1,0], x_k[2,0]],
             [x_k[4,0], x_k[5,0], x_k[6,0]],
             [x_k[8,0], x_k[9,0], x_k[10,0]]])
U, sigma, VT = la.svd(R_old)
R_new = U * VT
x_k[0,0], x_k[1,0], x_k[2,0] = R_new[0,0], R_new[0,1], R_new[0,2]
x_k[4,0], x_k[5,0], x_k[6,0] = R_new[1,0], R_new[1,1], R_new[1,2]
x_k[8,0], x_k[9,0], x_k[10,0] = R_new[2,0], R_new[2,1], R_new[2,2]
```

    step = 1, abs(mse-mse_last) = 9.83445441
    step = 2, abs(mse-mse_last) = 0.07658973
    step = 3, abs(mse-mse_last) = 0.03711521
    step = 4, abs(mse-mse_last) = 0.00000000
    step =  4
    mse =  [[9.72074947]]

```python
# visualize keypoint after BA
# compare with image_kp_ABCDEF
print(x_k)
getfx_final = getFx(x_k, frame)
print(getfx_final)
plt.figure()
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[0,0], getfx_final[1,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[2,0], getfx_final[3,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[4,0], getfx_final[5,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[6,0], getfx_final[7,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[8,0], getfx_final[9,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[10,0], getfx_final[11,0], s = 80, c = 'r', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF_original_opt') # 图像题目
plt.show()
```

```python
def getValue(X3D, para):
    j = 0
    X3D_value = X3D.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
            r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
            r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
            al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
    return X3D_value
```

```python
# visualize 3D points and groundtruth
# XA,XB,XC,XD,XE,XF with GT[14,8,11,4,0,1]
para = x_k.copy()
K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                 [0.0, 1147.59161666764, 508.064917088557],
                 [0.0, 0.0, 1.0]])
r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
a, b, c, th, al = symbols('a b c th al')
ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
XB = Matrix([[0], [c], [0], [1]])
XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
XD = Matrix([[-b * c], [0], [0], [1]])
XE = Matrix([[0], [0], [0], [1]])
XF = Matrix([[b * c], [0], [0], [1]])
A3D = Rt * XA
B3D = Rt * XB
C3D = Rt * XC
D3D = Rt * XD
E3D = Rt * XE
F3D = Rt * XF
j = 0
#A3D_value = A3D.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
#            r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
#            r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
#            al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
Y = mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T
s = Y[2,0]
print(s)
A3D_value = getValue(A3D, para) * s
B3D_value = getValue(B3D, para) * s
C3D_value = getValue(C3D, para) * s
D3D_value = getValue(D3D, para) * s
E3D_value = getValue(E3D, para) * s
F3D_value = getValue(F3D, para) * s
'''
E3D_value = (getValue(E3D, para) - getValue(E3D, para))*s
A3D_value = (getValue(A3D, para) - getValue(E3D, para))*s
B3D_value = (getValue(B3D, para) - getValue(E3D, para))*s
C3D_value = (getValue(C3D, para) - getValue(E3D, para))*s
D3D_value = (getValue(D3D, para) - getValue(E3D, para))*s
F3D_value = (getValue(F3D, para) - getValue(E3D, para))*s
'''
print(A3D_value)
print(B3D_value)
print(C3D_value)
print(D3D_value)
print(E3D_value)
print(F3D_value)
```

    5431.021804471869
    Matrix([[-110.910895781011], [-1009.10636889366], [5393.53820265049]])
    Matrix([[65.7564671098120], [-1014.95267461836], [5433.37327752853]])
    Matrix([[242.423830000635], [-1020.79898034306], [5473.20835240656]])
    Matrix([[-88.3715849144842], [-549.414059608507], [5423.71935115701]])
    Matrix([[35.2567551853507], [-541.415115889804], [5431.02180447187]])
    Matrix([[158.885095285186], [-533.416172171102], [5438.32425778673]])

```python
Y = mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T
print(Y[2,0])
X1 = mat([img1_gt[0][14], img1_gt[1][14], img1_gt[2][14]]).T
X2 = mat([img1_gt[0][8], img1_gt[1][8], img1_gt[2][8]]).T
X3 = mat([img1_gt[0][11], img1_gt[1][11], img1_gt[2][11]]).T
X4 = mat([img1_gt[0][4], img1_gt[1][4], img1_gt[2][4]]).T
X5 = mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T
X6 = mat([img1_gt[0][1], img1_gt[1][1], img1_gt[2][1]]).T
print(X1, X2, X3, X4, X5, X6)
'''
#s = 5340.55881868
X1 = (mat([img1_gt[0][14], img1_gt[1][14], img1_gt[2][14]]).T-Y)
print(X1)
X2 = (mat([img1_gt[0][8], img1_gt[1][8], img1_gt[2][8]]).T-Y)
print(X2)
X3 = (mat([img1_gt[0][11], img1_gt[1][11], img1_gt[2][11]]).T-Y)
print(X3)
X4 = (mat([img1_gt[0][4], img1_gt[1][4], img1_gt[2][4]]).T-Y)
print(X4)
X5 = (mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T-Y)
print(X5)
X6 = (mat([img1_gt[0][1], img1_gt[1][1], img1_gt[2][1]]).T-Y)
print(X6)
'''
```

    5431.021804471869
    [[-116.13794622]
     [-981.55022671]
     [5337.56816021]] [[   58.10612376]
     [-1032.54818762]
     [ 5312.38145874]] [[ 229.57596842]
     [-974.23751811]
     [5340.55881868]] [[ -88.47243168]
     [-551.69897883]
     [5430.16427386]] [[  35.17832488]
     [-541.48752095]
     [5431.02180447]] [[ 158.81323565]
     [-531.08752521]
     [5431.94416176]]

    '\n#s = 5340.55881868\nX1 = (mat([img1_gt[0][14], img1_gt[1][14], img1_gt[2][14]]).T-Y)\nprint(X1)\nX2 = (mat([img1_gt[0][8], img1_gt[1][8], img1_gt[2][8]]).T-Y)\nprint(X2)\nX3 = (mat([img1_gt[0][11], img1_gt[1][11], img1_gt[2][11]]).T-Y)\nprint(X3)\nX4 = (mat([img1_gt[0][4], img1_gt[1][4], img1_gt[2][4]]).T-Y)\nprint(X4)\nX5 = (mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T-Y)\nprint(X5)\nX6 = (mat([img1_gt[0][1], img1_gt[1][1], img1_gt[2][1]]).T-Y)\nprint(X6)\n'

```python
# 绘制散点图
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(X1[0], X1[1], X1[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X2[0], X2[1], X2[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X3[0], X3[1], X3[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X4[0], X4[1], X4[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X5[0], X5[1], X5[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X6[0], X6[1], X6[2], s = 80, c = 'g', marker = 'X')
ax.scatter(A3D_value[0], A3D_value[1], A3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(B3D_value[0], B3D_value[1], B3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(C3D_value[0], C3D_value[1], C3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(D3D_value[0], D3D_value[1], D3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(E3D_value[0], E3D_value[1], E3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(F3D_value[0], F3D_value[1], F3D_value[2], s = 80, c = 'r', marker = 'X')

ax.set_zlabel('Z', fontdict={'size': 15, 'color': 'red'})
ax.set_ylabel('Y', fontdict={'size': 15, 'color': 'red'})
ax.set_xlabel('X', fontdict={'size': 15, 'color': 'red'})
plt.show()
```

```python
# 计算3D error
d1 = A3D_value-X1
e1 = sum(map(lambda x:x**2, d1))
d2 = B3D_value-X2
e2 = sum(map(lambda x:x**2, d2))
d3 = C3D_value-X3
e3 = sum(map(lambda x:x**2, d3))
d4 = D3D_value-X4
e4 = sum(map(lambda x:x**2, d4))
d5 = E3D_value-X5
e5 = sum(map(lambda x:x**2, d5))
d6 = F3D_value-X6
e6 = sum(map(lambda x:x**2, d6))
error = (e1**(0.5)+e2**(0.5)+e3**(0.5)+e4**(0.5)+e5**(0.5)+e6**(0.5))/6
print(error)
print(d1,d2,d3,d4,d5,d6)
```

    56.6692591685530
    Matrix([[5.22705044046768], [-27.5561421819614], [55.9700424388993]]) Matrix([[7.65034334947030], [17.5955130062730], [120.991818785437]]) Matrix([[12.8478615774326], [-46.5614622292373], [132.649533729206]]) Matrix([[0.100846761598120], [2.28491922030128], [-6.44492270138107]]) Matrix([[0.0784303060919385], [0.0724050617569674], [0]]) Matrix([[0.0718596378686982], [-2.32864695846297], [6.38009602800958]])

```python

```


```python
import os, sys
import h5py
import numpy as np
from scipy.io import loadmat
import cv2
import matplotlib
%matplotlib inline
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from numpy import matrix as mat
from sympy import *
from numpy import linalg as la
```

```python
def getFx(para, frame):   # para中为(13*frame+3)的一套参数，frame传进来是为了确定循环次数
    #先写出参数表达式，ABDCDEF六个点的齐次坐标
    K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                     [0.0, 1147.59161666764, 508.064917088557],
                     [0.0, 0.0, 1.0]])
    r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
    Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
    a, b, c, th, al = symbols('a b c th al')
    ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
    f = Symbol('f')
    XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
    XB = Matrix([[0], [c], [0], [1]])
    XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
    XD = Matrix([[-b * c], [0], [0], [1]])
    XE = Matrix([[0], [0], [0], [1]])
    XF = Matrix([[b * c], [0], [0], [1]])
    ua, va, wa = K[0,:] * (Rt * XA), K[1,:] * (Rt * XA), K[2,:] * (Rt * XA)
    ub, vb, wb = K[0,:] * (Rt * XB), K[1,:] * (Rt * XB), K[2,:] * (Rt * XB)
    uc, vc, wc = K[0,:] * (Rt * XC), K[1,:] * (Rt * XC), K[2,:] * (Rt * XC)
    ud, vd, wd = K[0,:] * (Rt * XD), K[1,:] * (Rt * XD), K[2,:] * (Rt * XD)
    ue, ve, we = K[0,:] * (Rt * XE), K[1,:] * (Rt * XE), K[2,:] * (Rt * XE)
    uf, vf, wf = K[0,:] * (Rt * XF), K[1,:] * (Rt * XF), K[2,:] * (Rt * XF)
    #根据每一帧的循环，提取出Rt的参数，K是公用的，代入参数写出3D坐标，并计算出u/w，v/w
    #写成f的形式，即按照六个点分块，每块里面有M帧
    getfx = mat(np.zeros((6*frame*2,1)))
    for i in range(6):
        for j in range(frame):
            if i == 0 :
                f = Matrix([ua/wa, va/wa])
            elif i == 1 :
                f = Matrix([ub/wb, vb/wb])
            elif i == 2 :
                f = Matrix([uc/wc, vc/wc])
            elif i == 3 :
                f = Matrix([ud/wd, vd/wd])
            elif i == 4 :
                f = Matrix([ue/we, ve/we])
            else:
                f = Matrix([uf/wf, vf/wf])
            f_value = f.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
                        r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
                        r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
                        al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
            getfx[i*frame*2+j*2] = f_value[0]
            getfx[i*frame*2+j*2+1] = f_value[1]
    #返回getfx值，2*frame*6 by 1
    return getfx
```

```python
def getJacobian(point, frame, para):
    # 用参数表示K，R矩阵
    focalx, focaly, px, py = symbols('focalx focaly px py')
    r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
    Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
    K = Matrix([[focalx, 0, px], [0, focaly, py], [0, 0, 1]])
    # KRt = K * Rt
    # 用参数表示ABCDEF六个点坐标
    a, b, c, th, al = symbols('a b c th al')
    ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
    f = Symbol('f')
    if point == 0 :
        XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
        ua, va, wa = K[0,:] * (Rt * XA), K[1,:] * (Rt * XA), K[2,:] * (Rt * XA)
        f = Matrix([ua/wa, va/wa])
    elif point == 1 :
        XB = Matrix([[0], [c], [0], [1]])
        ub, vb, wb = K[0,:] * (Rt * XB), K[1,:] * (Rt * XB), K[2,:] * (Rt * XB)
        f = Matrix([ub/wb, vb/wb])
    elif point == 2 :
        XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
        uc, vc, wc = K[0,:] * (Rt * XC), K[1,:] * (Rt * XC), K[2,:] * (Rt * XC)
        f = Matrix([uc/wc, vc/wc])
    elif point == 3 :
        XD = Matrix([[-b * c], [0], [0], [1]])
        ud, vd, wd = K[0,:] * (Rt * XD), K[1,:] * (Rt * XD), K[2,:] * (Rt * XD)
        f = Matrix([ud/wd, vd/wd])
    elif point == 4 :
        XE = Matrix([[0], [0], [0], [1]])
        ue, ve, we = K[0,:] * (Rt * XE), K[1,:] * (Rt * XE), K[2,:] * (Rt * XE)
        f = Matrix([ue/we, ve/we])
    elif point == 5:
        XF = Matrix([[b * c], [0], [0], [1]])
        uf, vf, wf = K[0,:] * (Rt * XF), K[1,:] * (Rt * XF), K[2,:] * (Rt * XF)
        f = Matrix([uf/wf, vf/wf])
    args = Matrix([r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33, th, al, a, b, c])
    f_X1 = f[0,:].jacobian(args)
    f_X2 = f[1,:].jacobian(args)
    JA = Matrix([f_X1, f_X2])   # 2 by 16 matrix
    JA_value = JA.subs({focalx:1149.676, focaly:1147.592, px:508.849, py:508.065, r11:para[13*frame], r12:para[13*frame+1],
                                 r13:para[13*frame+2], r14:para[13*frame+3], r21:para[13*frame+4], r22:para[13*frame+5],
                                 r23:para[13*frame+6], r24:para[13*frame+7], r31:para[13*frame+8], r32:para[13*frame+9],
                                 r33:para[13*frame+10], th:para[13*frame+11], al:para[13*frame+12], a:para[-3], b:para[-2], c:para[-1]})
    #JA_value = JA_value.subs({f:1149.68})
    return JA_value
```

```python
def getJ(para, frame):
    getj = mat(np.zeros((6*frame*2, 13*frame+3)))
    for m in range(6):
        for n in range(frame):
            JA_value = getJacobian(m, n, para)
            #print(JA_value)
            getj[2*(m*frame+n):2*(m*frame+n+1), 13*n:13*n+13] = JA_value[:, 0:13]
            getj[2*(m*frame+n):2*(m*frame+n+1), -3:] = JA_value[:, -3:]
    return getj
```

```python
def getE(getfx, frame):
    E = mat(np.zeros((6*frame*2,1)))
    for i in range(6):
        for j in range(frame):
            if i==0 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==1 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==2 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==3 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==4 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
            elif i==5 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - x2d[i,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - x2d[i, 1]
    return E
```

```python
def LM_opti(frame, x_para, u=1, v=2, step_max=500):
    J = mat(np.zeros((6*frame*2, 13*frame+3)))
    E = mat(np.zeros((6*frame*2,1)))   # E = f(X) - b ;
    E_temp = mat(np.zeros((6*frame*2,1)))   # E_temp compare with E in L-M
    x_k = mat(x_para.copy())   #parameter initialization
    step = 0   # iteration steps
    mse_last = 0   # mse value after iteration each time
    step_max = 500   # maximum number of iteration
    u = 1
    v = 2   # u, v initial value
    # L-M Algorithm obtain optimal parameters
    while(step < step_max):
        step += 1
        mse, mse_temp = 0, 0

        # generate Jacobian Matrix and calculate E
        getfx = mat(np.zeros((6*frame*2,1)))
        getfx = getFx(x_k, frame)
        E = getE(getfx, frame)

        for i in range(6*frame*2):
            mse += E[i]**2
        mse /= 6*frame*2

        # get new J
        J = mat(np.zeros((6*frame*2, 13*frame+3)))
        J = getJ(x_k, frame)

        # delta X = ...
        #print(J.T * J)
        dx = mat(np.zeros((13*frame+3,1)))
        LM = u * mat(np.eye(13*frame+3))
        dx = -(J.T * J + LM).I * J.T * E
        x_k_temp = x_k.copy()
        x_k_temp += dx

        #R的更新不能简单赋值
        #get R meet R.T*R=I
        #U * D * V.T = R --> R+ = U * V.T
        R_old = mat([[x_k_temp[0,0], x_k_temp[1,0], x_k_temp[2,0]],
                     [x_k_temp[4,0], x_k_temp[5,0], x_k_temp[6,0]],
                     [x_k_temp[8,0], x_k_temp[9,0], x_k_temp[10,0]]])
        U, sigma, VT = la.svd(R_old)
        R_new = U * VT
        x_k_temp[0,0], x_k_temp[1,0], x_k_temp[2,0] = R_new[0,0], R_new[0,1], R_new[0,2]
        x_k_temp[4,0], x_k_temp[5,0], x_k_temp[6,0] = R_new[1,0], R_new[1,1], R_new[1,2]
        x_k_temp[8,0], x_k_temp[9,0], x_k_temp[10,0] = R_new[2,0], R_new[2,1], R_new[2,2]
        ###########

        # calculate E_temp with x_k_temp
        # copy from E with x_k
        getfx_temp = mat(np.zeros((6*frame*2,1)))
        getfx_temp = getFx(x_k_temp, frame)
        E_temp = getE(getfx_temp, frame)

        for i in range(6*frame*2):
            mse_temp += E_temp[i]**2
        mse_temp /= 6*frame*2

        # segma value to choose optimization model
        segma = (mse - mse_temp)/((dx.T * (u * dx - J.T * E))[0,0])

        # calculate new u
        if segma > 0:
            s = 1.0/3.0
            v = 2
            x_k = x_k_temp
            mse = mse_temp
            u = u * max(s, 1-pow(2*segma,3))
            u = u[0,0]
        else:
            u = u * v
            v = v * 2
            x_k = x_k_temp

        print("step = %d, abs(mse-mse_last) = %.8f" %(step, abs(mse-mse_last)))
        if abs(mse-mse_last)<0.000001:
            break
        mse_last = mse

    print("step = ", step)
    print("mse = ", mse_last)
    #print("parameter = ", x_k)

    return x_k
```

```python
# 数据读取
frame = 1
m = loadmat("valid.mat")
# camera intrinsic matrix
K = m["annot"][0][0][4]
K_cam = K[0][0].tolist()
# key point 3D groundtruth
gt = m["annot"][0][0][3]
img1_gt = gt[135]   # array 3 by 17
kp = np.zeros((17,2))
for i in range(17):
    u = K_cam[0] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    v = K_cam[1] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    w = K_cam[2] * mat([img1_gt[0][i], img1_gt[1][i], img1_gt[2][i]]).T
    kp[i][0] = u/w
    kp[i][1] = v/w
# load  and show image
img = cv2.imread("S9_Posing_1.55011271_000676.jpg")
plt.figure("Image") # 图像窗口名称
plt.imshow(img[:,:,[2,1,0]])
plt.axis('on') # 关掉坐标轴为 off
plt.title('image1') # 图像题目
plt.show()
# visualize key points
txt = ['1','2','3','4','5','6','7','8','9','10','11','12','13','14','15','16','17']
img_kp = plt.scatter(kp[:,0], kp[:,1], s = 80, c = 'g', marker = 'X')
for i in range(17):
    plt.annotate(txt[i], xy = (kp[i,0], kp[i,1]), xytext = (kp[i,0]+0.1, kp[i,1]+0.1)) # 这里xy是需要标记的坐标，xytext是对应的标签坐标
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp') # 图像题目
# visualize ABCDEF
plt.figure()
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF') # 图像题目
plt.show()
# save 2D coordinate to list
x2d = np.zeros((6 * frame,2))
x2d[0,0] = kp[0,0]
for i in range(6):
    for j in range(frame):
        if i==0 :
            x2d[i*frame+j, 0] = kp[14, 0]
            x2d[i*frame+j, 1] = kp[14, 1]
        elif i==1 :
            x2d[i*frame+j, 0] = kp[8, 0]
            x2d[i*frame+j, 1] = kp[8, 1]
        elif i==2 :
            x2d[i*frame+j, 0] = kp[11, 0]
            x2d[i*frame+j, 1] = kp[11, 1]
        elif i==3 :
            x2d[i*frame+j, 0] = kp[4, 0]
            x2d[i*frame+j, 1] = kp[4, 1]
        elif i==4 :
            x2d[i*frame+j, 0] = kp[0, 0]
            x2d[i*frame+j, 1] = kp[0, 1]
        elif i==5 :
            x2d[i*frame+j, 0] = kp[1, 0]
            x2d[i*frame+j, 1] = kp[1, 1]
print(x2d)
```

```python
# parameter initialization for all frame (K_cam, x2d(6*frame by 2))
# x_para(13*frame+3)
x_para = np.zeros((13*frame+3,1))
for i in range(frame):
    x_para[13*i] = -1   # r11
    x_para[13*i+5] = 1   # r22
    x_para[13*i+10] = -1   # r33
    x_para[13*i+3] = 0.0047
    x_para[13*i+7] = -0.0997
    x_para[13*i+11] = 0   # th
    x_para[13*i+12] = 0   #al
x_para[-3] = 0.35   # a
x_para[-2] = 0.25   # b
distance = -0.095#0.096   # c
x_para[-1] = distance
print(mat(x_para.copy()))
getfx_ini = getFx(x_para, frame)
print(getfx_ini)
```

    [[-1.    ]
     [ 0.    ]
     [ 0.    ]
     [ 0.0047]
     [ 0.    ]
     [ 1.    ]
     [ 0.    ]
     [-0.0997]
     [ 0.    ]
     [ 0.    ]
     [-1.    ]
     [ 0.    ]
     [ 0.    ]
     [ 0.35  ]
     [ 0.25  ]
     [-0.095 ]]
    [[476.02538041]
     [284.62882932]
     [514.25209744]
     [284.62882932]
     [552.47881446]
     [284.62882932]
     [486.94729956]
     [393.65003291]
     [514.25209744]
     [393.65003291]
     [541.55689531]
     [393.65003291]]

```python
plt.figure()
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[0,0], getfx_ini[1,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[2,0], getfx_ini[3,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[4,0], getfx_ini[5,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[6,0], getfx_ini[7,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[8,0], getfx_ini[9,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_ini[10,0], getfx_ini[11,0], s = 80, c = 'r', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF_original_opt') # 图像题目
plt.show()
```

```python
x_k = LM_opti(frame, x_para)
R_old = mat([[x_k[0,0], x_k[1,0], x_k[2,0]],
             [x_k[4,0], x_k[5,0], x_k[6,0]],
             [x_k[8,0], x_k[9,0], x_k[10,0]]])
U, sigma, VT = la.svd(R_old)
R_new = U * VT
x_k[0,0], x_k[1,0], x_k[2,0] = R_new[0,0], R_new[0,1], R_new[0,2]
x_k[4,0], x_k[5,0], x_k[6,0] = R_new[1,0], R_new[1,1], R_new[1,2]
x_k[8,0], x_k[9,0], x_k[10,0] = R_new[2,0], R_new[2,1], R_new[2,2]
```

    step = 1, abs(mse-mse_last) = 9.83445441
    step = 2, abs(mse-mse_last) = 0.07658973
    step = 3, abs(mse-mse_last) = 0.03711521

```python
# visualize keypoint after BA
# compare with image_kp_ABCDEF
print(x_k)
getfx_final = getFx(x_k, frame)
print(getfx_final)
plt.figure()
img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[0,0], getfx_final[1,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[8,0], kp[8,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[2,0], getfx_final[3,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[11,0], kp[11,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[4,0], getfx_final[5,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[4,0], kp[4,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[6,0], getfx_final[7,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[0,0], kp[0,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[8,0], getfx_final[9,0], s = 80, c = 'r', marker = 'X')
img_kp = plt.scatter(kp[1,0], kp[1,1], s = 80, c = 'g', marker = 'X')
img_kp = plt.scatter(getfx_final[10,0], getfx_final[11,0], s = 80, c = 'r', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('image_kp_ABCDEF_original_opt') # 图像题目
plt.show()
```

```python
def getValue(X3D, para):
    j = 0
    X3D_value = X3D.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
            r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
            r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
            al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
    return X3D_value
```

```python
# visualize 3D points and groundtruth
# XA,XB,XC,XD,XE,XF with GT[14,8,11,4,0,1]
para = x_k.copy()
K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                 [0.0, 1147.59161666764, 508.064917088557],
                 [0.0, 0.0, 1.0]])
r11, r12, r13, r14, r21, r22, r23, r24, r31, r32, r33 = symbols('r11 r12 r13 r14 r21 r22 r23 r24 r31 r32 r33')
Rt = Matrix([[r11, r12, r13, r14], [r21, r22, r23, r24], [r31, r32, r33, 1]])
a, b, c, th, al = symbols('a b c th al')
ua, va, wa, ub, vb, wb, uc, vc, wc, ud, vd, wd, ue, ve, we, uf, vf, wf = symbols('ua va wa ub vb wb uc vc wc ud vd wd ue ve we uf vf wf')
XA = Matrix([[-a * c * cos(th) * cos(al)], [c-a * c * sin(th)], [-a * c *  cos(th) * sin(al)], [1]])
XB = Matrix([[0], [c], [0], [1]])
XC = Matrix([[a * c * cos(th) * cos(al)], [c+a * c * sin(th)], [a * c * cos(th) * sin(al)], [1]])
XD = Matrix([[-b * c], [0], [0], [1]])
XE = Matrix([[0], [0], [0], [1]])
XF = Matrix([[b * c], [0], [0], [1]])
A3D = Rt * XA
B3D = Rt * XB
C3D = Rt * XC
D3D = Rt * XD
E3D = Rt * XE
F3D = Rt * XF
j = 0
#A3D_value = A3D.subs({r11:para[13*j], r12:para[13*j+1], r13:para[13*j+2], r14:para[13*j+3],
#            r21:para[13*j+4], r22:para[13*j+5], r23:para[13*j+6], r24:para[13*j+7],
#            r31:para[13*j+8], r32:para[13*j+9], r33:para[13*j+10], th:para[13*j+11],
#            al:para[13*j+12], a:para[-3], b:para[-2], c:para[-1]})
s = 5340.55881868
E3D_value = (getValue(E3D, para) - getValue(E3D, para))*s
A3D_value = (getValue(A3D, para) - getValue(E3D, para))*s
B3D_value = (getValue(B3D, para) - getValue(E3D, para))*s
C3D_value = (getValue(C3D, para) - getValue(E3D, para))*s
D3D_value = (getValue(D3D, para) - getValue(E3D, para))*s
F3D_value = (getValue(F3D, para) - getValue(E3D, para))*s
print(A3D_value)
print(B3D_value)
print(C3D_value)
print(D3D_value)
print(E3D_value)
print(F3D_value)
```

    Matrix([[-143.732977969521], [-459.901052798691], [-36.8592481249503]])
    Matrix([[29.9916868960575], [-465.649978271430], [2.31230523863426]])
    Matrix([[203.716351761636], [-471.398903744170], [41.4838586022188]])
    Matrix([[-121.569097994654], [-7.86570758781875], [-7.18081842657119]])
    Matrix([[0], [0], [0]])
    Matrix([[121.569097994654], [7.86570758781875], [7.18081842657119]])

```python
Y = mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T
#s = 5340.55881868
X1 = (mat([img1_gt[0][14], img1_gt[1][14], img1_gt[2][14]]).T-Y)
print(X1)
X2 = (mat([img1_gt[0][8], img1_gt[1][8], img1_gt[2][8]]).T-Y)
print(X2)
X3 = (mat([img1_gt[0][11], img1_gt[1][11], img1_gt[2][11]]).T-Y)
print(X3)
X4 = (mat([img1_gt[0][4], img1_gt[1][4], img1_gt[2][4]]).T-Y)
print(X4)
X5 = (mat([img1_gt[0][0], img1_gt[1][0], img1_gt[2][0]]).T-Y)
print(X5)
X6 = (mat([img1_gt[0][1], img1_gt[1][1], img1_gt[2][1]]).T-Y)
print(X6)
```

    [[-151.3162711 ]
     [-440.06270576]
     [ -93.45364426]]
    [[  22.92779888]
     [-491.06066667]
     [-118.64034573]]
    [[ 194.39764354]
     [-432.74999716]
     [ -90.46298579]]
    [[-123.65075656]
     [ -10.21145788]
     [  -0.85753061]]
    [[0.]
     [0.]
     [0.]]
    [[123.63491077]
     [ 10.39999574]
     [  0.92235729]]

```python
# 绘制散点图
fig = plt.figure()
ax = Axes3D(fig)
ax.scatter(X1[0], X1[1], X1[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X2[0], X2[1], X2[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X3[0], X3[1], X3[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X4[0], X4[1], X4[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X5[0], X5[1], X5[2], s = 80, c = 'g', marker = 'X')
ax.scatter(X6[0], X6[1], X6[2], s = 80, c = 'g', marker = 'X')
ax.scatter(A3D_value[0], A3D_value[1], A3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(B3D_value[0], B3D_value[1], B3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(C3D_value[0], C3D_value[1], C3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(D3D_value[0], D3D_value[1], D3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(E3D_value[0], E3D_value[1], E3D_value[2], s = 80, c = 'r', marker = 'X')
ax.scatter(F3D_value[0], F3D_value[1], F3D_value[2], s = 80, c = 'r', marker = 'X')

ax.set_zlabel('Z', fontdict={'size': 15, 'color': 'red'})
ax.set_ylabel('Y', fontdict={'size': 15, 'color': 'red'})
ax.set_xlabel('X', fontdict={'size': 15, 'color': 'red'})
plt.show()
```

```python

```