```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中为(8*frame)的一套参数，frame传进来是为了确定循环次数
    #先写出参数表达式，ABDCDEF六个点的齐次坐标
    K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                     [0.0, 1147.59161666764, 508.064917088557],
                     [0.0, 0.0, 1.0]])
    w, x, y, z, tx, ty= symbols('w x y z tx ty')
    Rt = Matrix([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w), tx],
                      [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w), ty],
                      [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y), 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({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095})
            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矩阵
    K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                     [0.0, 1147.59161666764, 508.064917088557],
                     [0.0, 0.0, 1.0]])
    w, x, y, z, tx, ty= symbols('w x y z tx ty')
    Rt = Matrix([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w), tx],
                    [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w), ty],
                      [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y), 1]])

    # 用参数表示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([x, y, z, w, tx, ty, th, al])
    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({x:para[8*frame], y:para[8*frame+1], z:para[8*frame+2], w:para[8*frame+3],
                                    tx:para[8*frame+4], ty:para[8*frame+5], th:para[8*frame+6],
                                    al:para[8*frame+7], a:0.35, b:0.25, c:-0.095})
    return JA_value
```

```python
def getJ(para, frame):
    getj = mat(np.zeros((6*frame*2, 8*frame)))
    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), 8*n:8*n+8] = JA_value[:, 0:8]
            #getj[2*(m*frame+n):2*(m*frame+n+1), -3:] = JA_value[:, -3:]
    return getj
```

```python
def getE(getfx, frame, k, xkp):
    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] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
            elif i==1 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
            elif i==2 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
            elif i==3 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
            elif i==4 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
            elif i==5 :
                E[(i*frame+j)*2] = getfx[i*frame*2+j*2] - xkp[i*3+k,0]
                E[(i*frame+j)*2+1] = getfx[i*frame*2+j*2+1] - xkp[i*3+k, 1]
    return E
```

```python
def LM_opti(frame, ith, x2d, x_para, u=1, v=2, step_max=500):
    J = mat(np.zeros((6*frame*2, 8*frame)))
    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

    # 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, ith, x2d)   #单帧模式，第几帧，GT

        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, 8*frame)))
        J = getJ(x_k, frame)

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

        #Q normalization
        for update_q in range(frame):
            q1, q2, q3, q4 = x_k_temp[8*update_q], x_k_temp[8*update_q+1], x_k_temp[8*update_q+2], x_k_temp[8*update_q+3]
            norm = sqrt(q1**2+q2**2+q3**2+q4**2)
            x_k_temp[8*update_q] = q1/norm
            x_k_temp[8*update_q+1] = q2/norm
            x_k_temp[8*update_q+2] = q3/norm
            x_k_temp[8*update_q+3] = q4/norm

        # 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, ith, x2d)

        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 = 3
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]
img_gt = np.zeros((frame, 3, 17))
img1_gt = gt[135]   # array 3 by 17
img2_gt = gt[136]   # array 3 by 17
img3_gt = gt[137]   # array 3 by 17
img_gt[0] = img1_gt
img_gt[1] = img2_gt
img_gt[2] = img3_gt
#print(img_gt)

# key point 2D groundtruth
kp = np.zeros((frame, 17, 2))
for m in range(frame):
    for i in range(17):
        u = K_cam[0] * mat([img_gt[m][0][i], img_gt[m][1][i], img_gt[m][2][i]]).T
        v = K_cam[1] * mat([img_gt[m][0][i], img_gt[m][1][i], img_gt[m][2][i]]).T
        w = K_cam[2] * mat([img_gt[m][0][i], img_gt[m][1][i], img_gt[m][2][i]]).T
        kp[m][i][0] = u/w
        kp[m][i][1] = v/w
#print(kp)

# show image
img1 = cv2.imread("S9_Posing_1.55011271_000676.jpg")
img2 = cv2.imread("S9_Posing_1.55011271_000681.jpg")
img3 = cv2.imread("S9_Posing_1.55011271_000686.jpg")
plt.figure(figsize = (15, 45)) # 图像窗口名称
plt.subplot(131)
plt.imshow(img1[:,:,[2,1,0]])
plt.subplot(132)
plt.imshow(img2[:,:,[2,1,0]])
plt.subplot(133)
plt.imshow(img3[:,:,[2,1,0]])
plt.axis('on') # 关掉坐标轴为 off
plt.title('image') # 图像题目
plt.show()

# visualize torso joints A-F
plt.figure(figsize = (5,15))
plt.subplot(311)
for i in {0,1,4,8,11,14}:
    img_kp = plt.scatter(kp[0][i,0], kp[0][i,1], s = 80, c = 'g', marker = 'X')
plt.subplot(312)
for i in {0,1,4,8,11,14}:
    img_kp = plt.scatter(kp[1][i,0], kp[1][i,1], s = 80, c = 'g', marker = 'X')
plt.subplot(313)
for i in {0,1,4,8,11,14}:
    img_kp = plt.scatter(kp[2][i,0], kp[2][i,1], s = 80, c = 'g', marker = 'X')
plt.axis('on') # 关掉坐标轴为 off
plt.title('torso keypoint of image') # 图像题目
plt.show()

# save 2D keypoint coordinate
x2d = np.zeros((6 * frame,2))   #[AAABBBCCCDDDEEEFFF].T
for i in range(6):
    for j in range(frame):
        if i==0 :
            x2d[i*frame+j, :] = kp[j][14, :]
        elif i==1 :
            x2d[i*frame+j, :] = kp[j][8, :]
        elif i==2 :
            x2d[i*frame+j, :] = kp[j][11, :]
        elif i==3 :
            x2d[i*frame+j, :] = kp[j][4, :]
        elif i==4 :
            x2d[i*frame+j, :] = kp[j][0, :]
        elif i==5 :
            x2d[i*frame+j, :] = kp[j][1, :]
print(x2d)
```

```python
# parameter initialization for first frame (x,y,z,w,tx,ty,th,al | a,b,c)
x_para = np.zeros((8,1))
x_para[0] = 0   # q(x)
x_para[1] = 1   # q(y)
x_para[2] = 0   # q(z)
x_para[3] = 0   # q(w)
x_para[4] = 0.0047   #tx(cal)
x_para[5] = -0.0997   #ty(cal)
x_para[6] = 0   # th
x_para[7] = 0   #al
#x_para[-3] = 0.35   # a
#x_para[-2] = 0.25   # b
#x_para[-1] = -0.095  # c(cal)
getfx_ini = getFx(x_para, 1)
print(getfx_ini)
```

    [[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
# visualize first frame after initilization
plt.figure()
img_kp = plt.scatter(kp[0][14,0], kp[0][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[0][8,0], kp[0][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[0][11,0], kp[0][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[0][4,0], kp[0][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,0], kp[0][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[0][1,0], kp[0][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
# optimization for each frame
X_para = np.zeros((8*frame,1))
#X_para[-3] = 0.35   # a
#X_para[-2] = 0.25   # b
#X_para[-1] = -0.095  # c(cal)
para_data = np.zeros((frame,8))   # save each frame's initialization parameter
x_k_para = x_para.copy()
for i in range(frame):
    x_k = LM_opti(1, i, x2d, x_k_para)   #单帧优化，第几帧，GT，初始化参数

    print("\nFor the No.", i+1, " frame : x_k = ", x_k)
    para_data[i] = mat(x_k).T
    # for next frame initilization
    x_k_para = x_k
    X_para[8*i:8*i+8] = x_k[0:8,0]   #save parameters for global Bundle Adjustment
print(para_data)
print(X_para)
```

    step = 1, abs(mse-mse_last) = 38.29289273
    step = 2, abs(mse-mse_last) = 28.39988047
    step = 3, abs(mse-mse_last) = 0.47630187
    step = 4, abs(mse-mse_last) = 0.06414766
    step = 5, abs(mse-mse_last) = 0.00000000
    step =  5
    mse =  [[9.35256273]]

    For the No. 1  frame : x_k =  [[-0.03172096]
     [ 0.98995575]
     [-0.12828207]
     [ 0.05025041]
     [ 0.00651697]
     [-0.09967906]
     [ 0.16674078]
     [ 0.18626365]]
    step = 1, abs(mse-mse_last) = 10.18696481
    step = 2, abs(mse-mse_last) = 0.00000000
    step =  2
    mse =  [[10.18696481]]

    For the No. 2  frame : x_k =  [[-2.41812822e-02]
     [ 9.88700196e-01]
     [-1.34147837e-01]
     [ 6.23822602e-02]
     [ 2.88641105e-04]
     [-9.97669594e-02]
     [ 1.84457688e-02]
     [-1.27532792e-01]]
    step = 1, abs(mse-mse_last) = 22.43796795
    step = 2, abs(mse-mse_last) = 11.16315841
    step = 3, abs(mse-mse_last) = 0.00000000
    step =  3
    mse =  [[11.27480954]]

    For the No. 3  frame : x_k =  [[-0.02587908]
     [ 0.98521556]
     [-0.15000481]
     [ 0.07860748]
     [-0.00738876]
     [-0.10002317]
     [ 0.12848023]
     [ 0.0086954 ]]
    [[-3.17209580e-02  9.89955750e-01 -1.28282072e-01  5.02504067e-02
       6.51697362e-03 -9.96790579e-02  1.66740785e-01  1.86263652e-01]
     [-2.41812822e-02  9.88700196e-01 -1.34147837e-01  6.23822602e-02
       2.88641105e-04 -9.97669594e-02  1.84457688e-02 -1.27532792e-01]
     [-2.58790838e-02  9.85215557e-01 -1.50004811e-01  7.86074789e-02
      -7.38876467e-03 -1.00023168e-01  1.28480228e-01  8.69540250e-03]]
    [[-3.17209580e-02]
     [ 9.89955750e-01]
     [-1.28282072e-01]
     [ 5.02504067e-02]
     [ 6.51697362e-03]
     [-9.96790579e-02]
     [ 1.66740785e-01]
     [ 1.86263652e-01]
     [-2.41812822e-02]
     [ 9.88700196e-01]
     [-1.34147837e-01]
     [ 6.23822602e-02]
     [ 2.88641105e-04]
     [-9.97669594e-02]
     [ 1.84457688e-02]
     [-1.27532792e-01]
     [-2.58790838e-02]
     [ 9.85215557e-01]
     [-1.50004811e-01]
     [ 7.86074789e-02]
     [-7.38876467e-03]
     [-1.00023168e-01]
     [ 1.28480228e-01]
     [ 8.69540250e-03]]

```python
def visual2D(kp, getfx):
    plt.figure()
    img_kp = plt.scatter(kp[14,0], kp[14,1], s = 80, c = 'g', marker = 'X')
    img_kp = plt.scatter(getfx[0,0], getfx[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[2,0], getfx[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[4,0], getfx[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[6,0], getfx[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[8,0], getfx[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[10,0], getfx[11,0], s = 80, c = 'r', marker = 'X')
    plt.axis('on') # 关掉坐标轴为 off
    plt.title('image_kp_ABCDEF_original_opt') # 图像题目
    plt.show()
```

```python
visual = np.zeros((8,1))
for i in range(frame):
    visual = mat(para_data[i]).T
    getfx_visual = getFx(visual, 1)
    visual2D(kp[i], getfx_visual)
```

```python
def visual3D(img_gt, para):
    K = Matrix([[1149.67569986785, 0.0, 508.848621645943],
                 [0.0, 1147.59161666764, 508.064917088557],
                 [0.0, 0.0, 1.0]])
    w, x, y, z, tx, ty= symbols('w x y z tx ty')
    Rt = Matrix([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w), tx],
                    [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w), ty],
                      [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y), 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
    Y = mat([img_gt[0][0], img_gt[1][0], img_gt[2][0]]).T
    s = Y[2,0]
    print(s)

    A3D_value = A3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    B3D_value = B3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    C3D_value = C3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    D3D_value = D3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    E3D_value = E3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    F3D_value = F3D.subs({x:para[8*j], y:para[8*j+1], z:para[8*j+2], w:para[8*j+3],
                                    tx:para[8*j+4], ty:para[8*j+5], th:para[8*j+6],
                                    al:para[8*j+7], a:0.35, b:0.25, c:-0.095}) * s
    print(A3D_value,B3D_value,C3D_value,D3D_value,E3D_value,F3D_value)

    X1 = mat([img_gt[0][14], img_gt[1][14], img_gt[2][14]]).T
    X2 = mat([img_gt[0][8], img_gt[1][8], img_gt[2][8]]).T
    X3 = mat([img_gt[0][11], img_gt[1][11], img_gt[2][11]]).T
    X4 = mat([img_gt[0][4], img_gt[1][4], img_gt[2][4]]).T
    X5 = mat([img_gt[0][0], img_gt[1][0], img_gt[2][0]]).T
    X6 = mat([img_gt[0][1], img_gt[1][1], img_gt[2][1]]).T
    print(X1, X2, X3, X4, X5, X6)

    # 绘制散点图
    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()

    # 计算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)
```

```python
visual3d = np.zeros((8,1))
for i in range(frame):
    visual3d = mat(para_data[i]).T
    visual3D(img_gt[i], visual3d)
```

```python

```

annot/BA and LM --- mF6P (R[q]) fix a:b:c.ipynb


# Neuromatch Academy: Week 1, Day 3, Tutorial 2
# Model Fitting: Linear regression with MLE

#Tutorial Objectives

This is Tutorial 2 of a series on fitting models to data. We start with simple linear regression, using least squares optimization (Tutorial 1) and Maximum Likelihood Estimation (Tutorial 2). We will use bootstrapping to build confidence intervals around the inferred linear model parameters (Tutorial 3). We'll finish our exploration of linear models by  generalizing to multiple linear regression (Tutorial 4). We then move on to polynomial regression (Tutorial 5). We end by learning how to choose between these various models. We discuss the bias-variance trade-off (Tutorial 6) and two common methods for model selection, AIC and Cross Validation (Tutorial 7).

In this tutorial, we will use a different approach to fit linear models that incorporates the random 'noise' in our data.
- Learn about probability distributions and probabilistic models
- Learn how to calculate the likelihood of our model parameters
- Learn how to implement the maximum likelihood estimator, to find the model parameter with the maximum likelihood

# Setup

```python
# @title  Imports
import matplotlib.pyplot as plt
import numpy as np
import scipy.stats as stats
import ipywidgets as widgets
```

```python
#@title Figure Settings
%matplotlib inline
fig_w, fig_h = (8, 6)
plt.rcParams.update({'figure.figsize': (fig_w, fig_h)})
%config InlineBackend.figure_format = 'retina'
```

```python
#@title Helper Functions
def plot_density_image(x, y, theta, sigma=1, ax=None):
  # plot the probability density of p(y|x,theta)
  if ax is None:
    fig, ax = plt.subplots()

  xmin, xmax = np.floor(np.min(x)), np.ceil(np.max(x))
  ymin, ymax = np.floor(np.min(y)), np.ceil(np.max(y))
  xx = np.linspace(xmin, xmax, 50)
  yy = np.linspace(ymin, ymax, 50)

  surface = np.zeros((len(yy), len(xx)))
  for i, x_i in enumerate(xx):
    surface[:, i] = stats.norm(theta*x_i, sigma).pdf(yy)

  ax.imshow(surface, origin='lower', aspect='auto', vmin=0, vmax=None,
            cmap=plt.get_cmap('Wistia'),
            extent=[xmin, xmax, ymin, ymax])
  ax.set(xlabel='x', ylabel='y')

def solve_normal_eqn(x, y):
  """Solve the normal equations to produce the value of theta_hat that minimizes
    MSE.

    Args:
    x (ndarray): An array of shape (samples,) that contains the input values.
    y (ndarray): An array of shape (samples,) that contains the corresponding
      measurement values to the inputs.
    thata_hat (float): An estimate of the slope parameter.

  Returns:
    float: The mean squared error of the data with the estimated parameter.
  """
  theta_hat = (x.T @ y) / (x.T @ x)
  return theta_hat
```

# Maximum Likelihood Estimation (MLE)

```python
#@title Video: Maximum Likelihood Estimation
from IPython.display import YouTubeVideo
video = YouTubeVideo(id="iBtB9kgloJ4", width=854, height=480, fs=1)
print("Video available at https://youtube.com/watch?v=" + video.id)
video
```

    Video available at https://youtube.com/watch?v=iBtB9kgloJ4

In the previous tutorial we took a first approximation assumption, that the data was drawn from a linear relationship with noise added, and found an effective approach for estimating model paramters based on minimizing the mean squared error.

In that case we treated the noise as simply a nuisance, but what if we factored it directly into our model?

Recall our linear model:

\begin{align}
y = \theta_0 + \theta_1 x + \eta.
\end{align}

The noise component $\eta$ is often modeled as a random variable drawn from a Gaussian distribution (also called the normal distribution).

The Gaussian distribution is described by its [probability density function](https://en.wikipedia.org/wiki/Probability_density_function) (pdf)
\begin{align}
\mathcal{N}(x; \mu, \sigma^2) = \frac{1}{\sqrt{2\pi\sigma^2}}e^{-\frac{1}{2\sigma^2}(x-\mu)^2}
\end{align}

and is dependent on two parameters: the mean $\mu$ and the variance $\sigma^2$. We often consider the noise signal to be "white noise" which is to say that it is Gaussian with zero mean and unit variance:

\begin{align}
\eta \sim \mathcal{N}(0, 1).
\end{align}

Use the explorer widget below to see how varying the $\mu$ and $\sigma$ parameters change the location and shape of the samples.

```python
#@title Gaussian Distribution Explorer
@widgets.interact(mu=widgets.FloatSlider(0.0, min=-2.0, max=2.0),
                  sigma=widgets.FloatSlider(1.0, min=0.5, max=2.0))
def plot_normal_dist(mu=0, sigma=1):
  rv = stats.norm(mu, sigma)
  x = np.linspace(-5, 5, 100)
  y = rv.pdf(x)
  samples = rv.rvs(1000)

  fig, ax = plt.subplots()
  ax.hist(samples, 20, density=True, histtype='stepfilled', alpha=0.8,
          label='samples')
  ax.plot(x, y, color='limegreen', linewidth=2, label='pdf')
  ax.vlines(mu, 0, rv.pdf(mu), colors='orange', label='$\mu$')
  ax.vlines([mu-sigma, mu+sigma], 0, rv.pdf([mu-sigma, mu+sigma]), colors='red',
            label='$\sigma$')
  ax.set(xlabel='x', ylabel='probability density', xlim=[-5, 5], ylim=[0, 1.0])
  ax.legend()
```

    interactive(children=(FloatSlider(value=0.0, description='mu', max=2.0, min=-2.0), FloatSlider(value=1.0, desc…

### Probabilistic Models

Now that we have a model of our noise component $\eta$ as random variable, how do we incorporate this back into our original linear model from before? Consider again our simplified model $y = \theta x + \eta$ where the noise has zero mean and unit variance $\eta \sim \mathcal{N}(0, 1)$. We can now also treat $y$ as a random variable drawn from a Gaussian distribution where $\mu = \theta x$ and $\sigma^2 = 1$:

\begin{align}
y \sim \mathcal{N}(\theta x, 1)
\end{align}

which is to say that the probability of observing $y$ given $x$ and parameter $\theta$ is
\begin{align}
p(y|x,\theta) = \frac{1}{\sqrt{2\pi}}e^{-\frac{1}{2}(y-\theta x)^2}.
\end{align}

---

Let's revist our original sample dataset where the true underlying model has $\theta = 1.2$.

```python
# setting a fixed seed to our random number generator ensures we will always
# get the same psuedorandom number sequence
np.random.seed(121)
theta = 1.2
n_samples = 30
x = 10*np.random.rand(n_samples) # sample from a uniform distribution over [0,10)
noise = np.random.randn(n_samples) # sample from a standard normal distribution
y = theta*x + noise
```

This time we can plot the density of $p(y|x,\theta=1.2)$ and see how $p(y)$ changes for different values of $x$.

```python
fig, (ax1, ax2) = plt.subplots(ncols=2, figsize=(10, 4))
# helper function to generate density image plots from data and parameters
plot_density_image(x, y, 1.2, ax=ax1)
ax1.axvline(8, color='k')

ylim = ax1.get_ylim()
yy = np.linspace(ylim[0], ylim[1], 50)
ax2.plot(yy, stats.norm(theta*8, 1).pdf(yy))
ax2.set(
    title='p(y|x=8,theta=1.2)',
    xlabel='y',
    ylabel='probability density');
```

### Likelihood Estimation

Now that we have our probabilistic model, we turn back to our original challenge of finding a good estimate for $\theta$ that fits our data. Given the inherent uncertainty when dealing in probabilities, we talk about the [likelihood](https://en.wikipedia.org/wiki/Likelihood_function) that some estimate $\hat \theta$ fits our data. The likelihood function $\mathcal{L(\theta)}$ is equal to the probability density function parameterized by that $\theta$:

\begin{align}
\mathcal{L}(\theta|x,y) = p(y|x,\theta) = \frac{1}{\sqrt{2\pi\sigma^2}}e^{-\frac{1}{2\sigma^2}(y-\theta x)^2}.
\end{align}

### Exercise: Likelihood Function

In this exercise you will implement the likelihood function $\mathcal{L}(\theta|x,y)$ for our linear model where $\sigma = 1$.

TIP: Use `np.exp` and `np.sqrt` for the exponential and square root functions, respectively.

```python
def likelihood(theta_hat, x, y):
  """The likelihood function for a linear model with noise sampled from a
    Gaussian distribution with zero mean and unit variance.

  Args:
    theta_hat (float): An estimate of the slope parameter.
    x (ndarray): An array of shape (samples,) that contains the input values.
    y (ndarray): An array of shape (samples,) that contains the corresponding
      measurement values to the inputs.

  Returns:
    ndarray: the likelihood values for the theta_hat estimate
  """
  sigma = 1
  ######################################################################
  ## TODO for students: implement the likelihood function
  ######################################################################
  pdf = 1 / np.sqrt(2 * np.pi * sigma ** 2) * np.exp(-(y - theta_hat * x) ** 2 / (2 * sigma ** 2))

  # comment this out when you've filled
  #raise NotImplementedError("Student excercise: implement the likelihood function")
  return pdf
```

```python
# to_remove solution
def likelihood(theta_hat, x, y):
  """The likelihood function for a linear model with noise sampled from a
    Gaussian distribution with zero mean and unit variance.

  Args:
    theta_hat (float): An estimate of the slope parameter.
    x (ndarray): An array of shape (samples,) that contains the input values.
    y (ndarray): An array of shape (samples,) that contains the corresponding
      measurement values to the inputs.

  Returns:
    float: the likelihood value for the theta_hat estimate
  """
  sigma = 1
  pdf = 1 / np.sqrt(2*np.pi*sigma**2) * np.exp(-(y - theta_hat*x)**2 / (2*sigma**2))
  return pdf
```

Now that we have our likelihood function, we can produce probabilities that our estimate $\hat{\theta}$ generated the provided observations. Let's try with one of the samples from our dataset.

```python
likelihood(1.0, x[1], y[1])
```

    0.11344443599846923

We should see that $\mathcal{L}(\theta=1.0|x=2.1,y=3.7) \approx 0.11$. So far so good, but how does this tell us how this estimate is better than any others?

When dealing with a set of data points, as we are with our dataset, we are concerned with their joint probability -- the likelihood that all data points are explained by our parameterization. Since we assume the noise affects each output independently, we can represent this as

\begin{align}
\mathcal{L}(\theta|x,y) = \prod_{i=1}^N \mathcal{L}(\theta|x_i,y_i).
\end{align}

Such a product is numerically unstable (multiplying small values together leads to underflow), so taking the logarithm of the likelihood helps by transforming products into sums:

\begin{align}
\operatorname{log}\mathcal{L}(\theta|x,y) = \sum_{i=1}^N \operatorname{log}\mathcal{L}(\theta|x_i,y_i)
\end{align}

We can take the sum of the log of the output of our `likelihood` method applied to the full dataset to get a better idea of how different $\hat\theta$ compare. We can also plot the different distribution densities over our dataset and see how they line up qualitatively.

```python
theta_hats = [0.5, 1.0, 2.2]
fig, axes = plt.subplots(ncols=3, figsize=(16, 4))
for theta_hat, ax in zip(theta_hats, axes):
  ll = np.sum(np.log(likelihood(theta_hat, x, y))) # log likelihood
  plot_density_image(x, y, theta_hat, ax=ax)
  ax.scatter(x, y)
  ax.set(title=f'theta_hat = {theta_hat}, log likelihood: {ll:.2f}')
```

Using the log likelihood calculation, we see that $\mathcal{L}(\theta=1.0) > \mathcal{L}(\theta=0.5) > \mathcal{L}(\theta=2.2)$.

This is great, now we have a way to compare estimators based on likelihood. But like with the MSE approach, we want an analytic solution to find the best estimator. In this case, we want to find the estimator that maximizes the likelihood.

### Finding the Maximum Likelihood Estimator

Our aim is to find some $\hat\theta$ that maximizes the likelihood function:

\begin{align}
\hat{\theta}_{\textrm{MLE}} = \underset{\theta}{\operatorname{argmax}} \mathcal{L}(\theta|x,y) = \underset{\theta}{\operatorname{argmax}}\frac{1}{\sqrt{2\pi\sigma^2}}e^{-\frac{1}{2\sigma^2}(y-\theta x)^2}.
\end{align}

We know from before that it's much nicer to work with the log likelihood:

\begin{align}
\hat{\theta}_{\textrm{MLE}} = \underset{\theta}{\operatorname{argmax}}\sum_{i=1}^N \operatorname{log}\mathcal{L}(\theta|x_i,y_i).
\end{align}

Substituting in our model and taking its logarithm, we get
\begin{align}
\hat{\theta}_{\textrm{MLE}} = \underset{\theta}{\operatorname{argmax}}-\frac{N}{2} \operatorname{log} 2\pi\sigma^2 - \frac{1}{2\sigma^2}\sum_{i=1}^N (y_i-\theta x_i)^2.
\end{align}

Now, we want to maximize the log likelihood, which is the same as minimizing the negative log likelihood. Just like our solution to minimizing MSE, we can take the derivative of our negative log likelihhood, set it to 0, and solve.

\begin{align}
\frac{\partial\operatorname{log}\mathcal{L}(\theta|x,y)}{\partial\theta}=\frac{1}{\sigma^2}\sum_{i=1}^N(y_i-\theta x_i)x_i = 0
\end{align}

This looks remarkably like the equation we had to solve for the optimal MSE estimator, and, in fact, we arrive to the exact same solution!

\begin{align}
\hat{\theta}_{\textrm{MLE}} = \hat{\theta}_{\textrm{MSE}} = \sum_{i=1}^N \frac{x_i y_i}{x_i^2}
\end{align}

```python
theta_hat_mle = (x @ y) / (x @ x)
fig, ax = plt.subplots()
ll = np.sum(np.log(likelihood(theta_hat_mle, x, y))) # log likelihood
plot_density_image(x, y, theta_hat_mle, ax=ax)
ax.scatter(x, y)
ax.set(title=f'theta_hat = {theta_hat_mle:.2f}, log likelihood: {ll:.2f}')
```

# Summary

- Likelihood vs probability
    - $\mathcal{L}(\theta|x, y) = p(y|\theta, x)$
    - $p(y|\theta, x)$ -> "probability of observing the response $y$ given parameter $\theta$ and input $x$"
    - $\mathcal{L}(\theta|x, y)$ -> "likelihood that parameters $\theta$ produced response $y$ from input $x$"
- Log-likelihood maximization
    - We take the $\textrm{log}$ of the likelihood function for computational convenience
    - The parameters $\theta$ that maximize $\textrm{log}\mathcal{L}(\theta|x, y)$ are the model parameters that maximize the probability of observing the data.
- **Key point**:
  - the log-likelihood is a flexible cost function, and is often used to find model parameters that best fit the data.

# Appendix

### General approach to maximum likelihood estimation

We want to find the parameter value that makes our data set most likely. We have $N$ data points $X = \{x_1,...,x_N\}$ and $Y = \{y_1,...,y_N\}$ and want:

$$\hat{\theta}_{\textrm{ML}} = \underset{\theta}{\operatorname{argmax}} p(Y |X, \theta)$$

We have assumed that the noise affects each output independently, we can therefore factorize as follows:
$$\hat{\theta}_{\textrm{ML}} = \underset{\theta}{\operatorname{argmax}}\prod_{i=1}^N p(y_i |x_i, \theta).$$

Optimizing such a product is numerically unstable (multiplying small values together leads to underflow), so taking the logarithm of the likelihood helps by transforming products into sums:

$$ \textrm{log} \prod_{i=1}^N p(y_i |x_i, \theta) = \sum_{i=1}^m \textrm{log} p(y_i |x_i, \theta)$$

The $\textrm{log}()$ function is *monotonically increasing*, which means that it preserves the order of its inputs. In practice optimiaztion routines are usually developed to solve minimization problem, we therefore rewrite our objective as a minimization of the negative log likelihood. Finally we obtain a problem that is equivalent to the original and more practical to solve:
$$\hat{\theta}_{\textrm{ML}} = \underset{\theta}{\operatorname{argmax}}\prod_{i=1}^m P_{\textrm{model}}(x_i | \theta) = \underset{\theta}{\operatorname{argmin}} -\sum_{i=1}^m \textrm{log}P_{\textrm{model}}(x_i | \theta).$$

---

### Probability vs likelihood

The expression $\mathrm{p}(\mathrm{y} | \mathrm{x}, \theta)$ can be considered as a function of $y$ or of $x$:

1. as a function of $y$, it is the probability distribution of the response conditioned on the input $x$ and the parameter $\theta$ (aka "encoding distribution")
  - Think of **probability** relating to possible **responses**

2. as a function of $\theta$, it is the probability of the data given the model parameter (aka likelihood function, which is not a proabbility distribution, meaning it usually does not sum to one).
  - Think of **likelihood** relating to **parameters**

In order to keep things clean, we can write the encoding distribution as $\mathrm{p}(\mathrm{y} | \mathrm{x}, \theta)$ and the likelihood function as $\mathcal{L}(\theta;x, y)$.

We can also see $\mathrm{p}(\mathrm{y} | \mathrm{x}, \theta)$ as a function of $x$. This is the stimulus likelihood function, and it is useful in case we want to decode the input $x$ from observed responses $y$. This is what is relevant from the point of view of a neuron that does not have access to the outside world and tries to infer what's out there from the responses of other neurons!

---