def distortion(m_focal_ideal, k):
    #输入：m_focal_ideal（理想焦平面上的齐次坐标点，H x W x 3矩阵），k（模型参数）
    #输出：m_focal_real（失真焦平面上的齐次坐标点，H x W x 3矩阵）

    kc = k[0]
    xi = k[1]

    X = (1/(m_focal_ideal[:,:,2:3]+xi*np.sqrt(m_focal_ideal[:,:,0:1]**2+m_focal_ideal[:,:,1:2]**2+m_focal_ideal[:,:,2:3]**2)))*m_focal_ideal[:,:,0:2]  # 从空间映射到标准平面

    k1 = kc[0]
    k2 = kc[1]
    k3 = kc[2]
    k4 = kc[3]
    k5 = kc[4]    
    
    m_focal_real = np.zeros_like(m_focal_ideal)
 
    x=X[:,:,0]
    y=X[:,:,1]
    r2=x**2+y**2
    radDist = 1. + k1*r2 + k2*(r2**2) + k5*(r2**3)
    m_focal_real[:,:,0] = x*radDist + 2*k3*x*y + k4*(r2+2*(x**2))
    m_focal_real[:,:,1] = y*radDist + k3*(r2+2*(y**2)) + 2*k4*x*y
    m_focal_real[:,:,2] = 1.
    
    return m_focal_real

# 相机校准参数
k = []
k.append(np.array([-0.616031774058559, 0.236026168622863, -0.0109419992705452, -0.00217955565809950, 0]))
k.append(1.745612606223418)

K_real = np.array([[1.825099190841841e+03, 0., 6.486113006422010e+02],[0., 1.817526262377727e+03, 4.911359689139596e+02],[0., 0., 1.]])

# 理想参数
K_ideal = np.array([[480, 0, 825],[0, 480, 460], [0, 0, 1]])
h_ideal = 900
w_ideal = 1600

import numpy as np  #导入NumPy库，用于数组和矩阵的数值计算
import matplotlib.pyplot as plt #导入Matplotlib库，用于数据和图像的可视化
from PIL import Image #从Python图像库（PIL）中导入Image模块，用于打开和处理图像
import math #提供数学函数

def interp2_bilinear(im, x, y): #在坐标x和y处对图像im执行双线性插值
    
    x0 = np.floor(x).astype(int) #对每个坐标计算其不大于x的最大整数(下取整)
    x1 = x0 + 1   #计算下一个整数
    y0 = np.floor(y).astype(int)
    y1 = y0 + 1
    
    #检查x0是否小于0或x1是否超出图像宽度/高度
    maskx = np.logical_or(x0<0, x1>im.shape[1]-1)
    masky = np.logical_or(y0<0, y1>im.shape[0]-1)
    #合并两个掩码，标识所有越界的坐标
    mask_notvalid = np.logical_or(maskx,masky)

    #将无效的x0索引设置为0
    x0[mask_notvalid] = 0
    x1[mask_notvalid] = 0
    y0[mask_notvalid] = 0
    y1[mask_notvalid] = 0
    
    
  
    if(len(im.shape)==3):  #检查图像维度：检查图像是否有三维
        Ia = im[ y0, x0, : ] #提取相邻像素(左上角的像素)
        Ib = im[ y1, x0, : ] #左下角
        Ic = im[ y0, x1, : ] #右上角
        Id = im[ y1, x1, : ] #右下角
        
        #计算插值权重
        wa = np.expand_dims((x1-x) * (y1-y),axis=Ia.ndim-1) #Ia的权重
        wb = np.expand_dims((x1-x) * (y-y0),axis=Ia.ndim-1)
        wc = np.expand_dims((x-x0) * (y1-y),axis=Ia.ndim-1)
        wd = np.expand_dims((x-x0) * (y-y0),axis=Ia.ndim-1)
    
    else: #处理具有二维的图像（灰度图像）
        Ia = im[ y0, x0]
        Ib = im[ y1, x0]
        Ic = im[ y0, x1]
        Id = im[ y1, x1]
        
        wa = ((x1-x) * (y1-y))
        wb = ((x1-x) * (y-y0))
        wc = ((x-x0) * (y1-y))
        wd = ((x-x0) * (y-y0))

#返回插值后的像素值和无效坐标的掩码
    return wa*Ia + wb*Ib + wc*Ic + wd*Id, mask_notvalid

I = np.array(Image.open('Tour_eiffel.jpg')).astype(float)/255.
h,w,_ = I.shape
fig1, axs1 = plt.subplots(ncols=1)
axs1.imshow(I)
axs1.set_title('原始图像')
plt.pause(0.1)

theta = 60 # 结果图像应顺时针旋转60°
center_rot = np.array([w/2., h/2.])#将旋转中心设置为图像的中心（宽度和高度的一半）

h_new = 1280
w_new = 960

def getInterpolationGrids(theta, center_rot, h_new, w_new):
    #计算用于旋转图像的坐标网格
    
    theta_rad = theta*math.pi/180.  #将旋转角度从度数转换为弧度
    rot_mat = np.array([[np.cos(theta_rad), -np.sin(theta_rad)],[np.sin(theta_rad), np.cos(theta_rad)]]) #构建给定角度的二维旋转矩阵
    
    #创建坐标网格
    x = np.arange(0,w_new)
    y = np.arange(0,h_new)
    X, Y = np.meshgrid(x,y) # h_new x w_new
    
    p = np.zeros((h_new,w_new,2)) #初始化一个数组来存储坐标对
    p[:,:,0] = X #分配x坐标
    p[:,:,1] = Y
    

    p_rot = ((p - np.array([w_new/2., h_new/2.])) @ rot_mat) + center_rot # h_new x w_new x 2    旋转方程 
    
    #提取旋转后的坐标
    XI = p_rot[:,:,0] 
    YI = p_rot[:,:,1] 

    return XI, YI

#调用函数获取用于插值的旋转网格
XI,YI = getInterpolationGrids(theta, center_rot, h_new, w_new)

fig2, axs2 = plt.subplots(ncols=2)
axs2[0].imshow(XI)
axs2[0].set_title('XI')
axs2[1].imshow(YI)
axs2[1].set_title('YI')
plt.pause(0.1)

I_rot,_ = interp2_bilinear(I.astype(np.float32), XI.astype(np.float32), YI.astype(np.float32))#执行双线性插值来计算旋转后的图像(转换为32位浮点格式)

h,w,_ = I.shape
fig3, axs3 = plt.subplots(ncols=2)
axs3[0].imshow(I)
axs3[0].set_title('原始图像')
axs3[1].imshow(I_rot)
axs3[1].set_title('变换后的图像')
plt.pause(1.)

Image.fromarray((I_rot*255).astype(np.uint8)).save("im_rot.bmp")

import numpy as np
from distortion import distortion

def getInterpolationGrids(K_reel, k_reel, K_ideal, h_ideal, w_ideal):
    #计算用于畸变校正图像
    
    #	生成理想图像平面的像素坐标网格
    x = np.arange(0,w_ideal)
    y = np.arange(0,h_ideal)
    X, Y = np.meshgrid(x,y)  # h_ideal x w_ideal
    
    #将像素坐标转换为齐次坐标并应用逆校准矩阵：
    p_ideal = np.ones((h_ideal,w_ideal,3)) #初始化一个数组来存储坐标对
    p_ideal[:,:,0] = X #分配x坐标
    p_ideal[:,:,1] = Y
    
 		K_ideal_inv = np.linalg.inv(K_ideal) #3x3
    
    m_focal_ideal = p_ideal @ (K_ideal_inv.T) #h_ideal x w_ideal x 3

    m_focal_distordu = distortion(m_focal_ideal, k_reel) #h_ideal x w_ideal x 3

    p_image_reel = m_focal_distordu @ (K_reel.T) #h_ideal x w_ideal x 3

    XI = p_image_reel[:,:,0] #h_ideal x w_ideal
    YI = p_image_reel[:,:,1] #h_ideal x w_ideal

    return XI, YI

# this code is for exercising the TP of vedio 3D ;the goal is to  restore a distorted image to a normal image

#做法很简单，先将一张理想图片变换到畸形图片，这个变换过程中我们就找到了理想图和畸变图之间坐标对应关系 
#知道了坐标对应关系就可以对畸变图像进行线性插值处理，即可变到理想图像


import numpy as np  # numerical calculation of arrays and matrices
import matplotlib.pyplot as plt # visualization of data and image
from PIL import Image #open and operate image
from interp2_bilinear import interp2_bilinear
import cv2
import time

#%% load parameters
# 我们需要提前知道的参数如下
# 第一是相机的内参矩阵，也就是m=K^-1*p中的K，这里是K，不是k。用于将相机平面P点转换到归一化焦平面中的m点
# 第二是畸变函数 这个函数用于将归一化焦平面中的点 进行畸变 来变换到畸变焦平面下的点  
# 第三是另一个K2 用于 将焦平面下的m_distortion 来变换到 相机平面下的p_distortion 对应公式 p_distortion = K2 * m_distortion

K_ideal = np.array([[480, 0, 825],[0, 480, 460], [0, 0, 1]]) # 从这个矩阵中可以看出相机的焦距为480
K_distortion = np.array([[1.825099190841841e+03, 0., 6.486113006422010e+02],[0., 1.817526262377727e+03, 4.911359689139596e+02],[0., 0., 1.]])


k_distortion = [] 
k_distortion.append(np.array([-0.616031774058559, 0.236026168622863, -0.0109419992705452, -0.00217955565809950, 0]))
k_distortion.append(1.745612606223418)

# 两个K已经导入了，接下来导入理想图片 但是注意，我们导入的并不是一个真的图片，而只是图片的纬度网格，因为我们并不关注像素值，而是关注于网格坐标的对应关系
# 因此构建理想图片的网格坐标(在已知图片大小的前提下)

h_ideal = 900
w_ideal = 1600

# 当我们有这些参数的时候 我们可以进行理想图像平面和畸形图像平面之间的坐标对应了，我们将使用一个函数来实现
# 这个函数的输入理应有五个，分别是 (K_ideal  K_distortion 畸变函数 h_ideal w_ideal)
# 现在的问题在于，畸变函数我们还没有给出，因此下面在编写坐标对应函数之前，先实现distortion函数

#%% distortion函数的实现  这部分函数的代码由老师在题目中提供
def distortion(m_focal_ideal, k):
    #输入：m_focal_ideal（理想焦平面上的齐次坐标点，H x W x 3矩阵），k（模型参数）
    #输出：m_focal_real（失真焦平面上的齐次坐标点，H x W x 3矩阵）

    kc = k[0]
    xi = k[1]

    X = (1/(m_focal_ideal[:,:,2:3]+xi*np.sqrt(m_focal_ideal[:,:,0:1]**2+m_focal_ideal[:,:,1:2]**2+m_focal_ideal[:,:,2:3]**2)))*m_focal_ideal[:,:,0:2]  # 从空间映射到标准平面

    k1 = kc[0]
    k2 = kc[1]
    k3 = kc[2]
    k4 = kc[3]
    k5 = kc[4]    
    
    m_focal_real = np.zeros_like(m_focal_ideal)
 
    x=X[:,:,0]
    y=X[:,:,1]
    r2=x**2+y**2
    radDist = 1. + k1*r2 + k2*(r2**2) + k5*(r2**3)
    m_focal_real[:,:,0] = x*radDist + 2*k3*x*y + k4*(r2+2*(x**2))
    m_focal_real[:,:,1] = y*radDist + k3*(r2+2*(y**2)) + 2*k4*x*y
    m_focal_real[:,:,2] = 1.
    
    return m_focal_real

# 我们对上述畸变函数代码进行稍微分析，我么有两个输入，分别是理想焦平面上的齐次坐标点m_ideal，另一个是畸变参数k  输出是畸变焦平面点的m_distortion

#%% 我们现在已经得到了畸变函数，坐标对应函数(这里起名为grid_corresponding)的五个输入量已经齐了，其输入位置的第五个参数 用畸变参数k来表现
def grid_corresponding(K_ideal,K_distortion,k_distortion,h_ideal,w_ideal):
    
    
    x = np.arange(0,w_ideal) # 生成从 0 到 w_ideal-1 的整数序列，长度为 w_ideal，代表图像在 x 方向的像素索引
    y = np.arange(0,h_ideal)
    X,Y=np.meshgrid(x,y)  # 生成了两个二维矩阵X和Y 大小为y*x  (注意纬度是反过来的) 也就是 行数(y)乘列数(x)
    #其中的X主要专注于行，每一行都复制了x(一维数组)的值    而Y专注于列，每一列都复制了y的值
    #举例
    # x = np.arange(0, 3)  # x 序列: [0, 1, 2]
    # y = np.arange(0, 2)  # y 序列: [0, 1]
    
    # X = [[0, 1, 2],
    #      [0, 1, 2]]

    # Y = [[0, 0, 0],
    #      [1, 1, 1]]
        
    
    p_ideal =np.ones((h_ideal,w_ideal,3))  # 利用理想图像的宽高来构建一个全1矩阵，用于将网格坐标填入  矩阵已经生成了，所以现在缺少网格坐标
    # 另外一个问题，为什么是全1矩阵，因为它的大小为 w_ideal* h_ideal*3 三通道矩阵，每个纬度分别用来储存X Y Z 由于是齐次坐标 所以Z=1
    p_ideal[:,:,0] = X #分配x坐标
    p_ideal[:,:,1] = Y 
    # 到这一步已经蒙了，下面写什么，不要急，先写出关键对应公式，缺什么补什么    可见缺p_ideal 在上面补上
   
    
   
    # 注意矩阵纬度 K_ideal是3*3  p_ideal是三维 ，但是其中每个像素点是一个 3* 1的齐次坐标 
    K_ideal_inv = np.linalg.inv(K_ideal) #3x3
    m_ideal = p_ideal @ (K_ideal_inv.T) #h_ideal x w_ideal x 3
    
    m_distortion = distortion(m_ideal, k_distortion)
    p_distortion = m_distortion @ (K_distortion.T)
    
    # 这里有个纬度的错误 导致错误的关键原因就是，不知道期望图片的尺寸是多少 ===> 900*1600 即y(代表有多少行) * x(代表有多少列)
    # 因此对于网格的构建顺序就是np.meshgrid(x,y)  对于p_ideal尺寸等价于图片尺寸 

    
    

    # 输出
    XI = p_distortion[:,:,0]
    YI = p_distortion[:,:,1]
    
    return XI,YI


#%% 函数定义完了 开始用
XI,YI =  grid_corresponding(K_ideal,K_distortion,k_distortion,h_ideal,w_ideal)

#输出显示
fig1, axs1 = plt.subplots(ncols=2)
axs1[0].imshow(XI)
axs1[1].imshow(YI)
plt.pause(0.1)

#%% load image to undistort
I_reel = np.array(Image.open('videoframe-3.bmp')).astype(float)/255.

fig2, axs2 = plt.subplots(ncols=2)
axs2[0].imshow(I_reel)
plt.pause(0.1)

a = time.time()
I_undist,_ = interp2_bilinear(I_reel.astype(np.float32), XI.astype(np.float32), YI.astype(np.float32))
print('{} sec'.format(time.time()-a))

axs2[1].imshow(I_undist)
plt.pause(0.1)

Image.fromarray((I_undist*255).astype(np.uint8)).save("im_undist.bmp")

I_undist, _ = interp2_bilinear(I_reel.astype(np.float32), XI.astype(np.float32), YI.astype(np.float32))

import numpy as np
import math

# 理想相机内参矩阵
K_ideal = np.array([[480, 0, 825],
                    [0, 480, 460],
                    [0,   0,   1]])

# 校正后图像尺寸
w_ideal = 1600  # 图像宽度（像素）
h_ideal = 900   # 图像高度（像素）

# 从内参矩阵中提取焦距
f = K_ideal[0, 0]


# 计算水平视场角（弧度）
theta_horizontal = 2 * np.arctan((w_ideal / 2) / f)

# 计算垂直视场角（弧度）
theta_vertical = 2 * np.arctan((h_ideal / 2) / f)

# 将视场角转换为度数
theta_horizontal_deg = np.degrees(theta_horizontal)
theta_vertical_deg = np.degrees(theta_vertical)

print("angle de champ horizontal(degre)：{:.2f}".format(theta_horizontal_deg))
print("angle de champ vertical(degre)：{:.2f}".format(theta_vertical_deg))

p_B = np.array([[..., ..., ..., ...],[..., ..., ..., ...]])

p_A = np.array([[..., ..., ..., ...],[..., ..., ..., ...]])

# 实验二的目标是将一个相机平面中的点投影到另一个相机平面中的点，换句话说，是两个平面场景图像的变换(homographie) 叫做单应性变换 描述两个图像之间的变换关系 换句话说 同一场景下不同视角图像
# 为了让B场景中的坐标点变换到A场景 公式描述如下 P_A(齐次坐标)=投影函数(H_AB * P_B(齐次坐标))
# 现在有两个东西要求 第一个是P_B的坐标点 
# 由于我们变换的是一张纸，因此我们取四个点，找到四个角的对应关系即可
# P_B坐标点好说，因为B相机平面是我们要生成的A4纸平面，即自定义平面大小，可以根据像素来得四角坐标
# 随后我们展开了齐次坐标，找相对应的非齐次坐标 因此构建了非齐次坐标的公式 即 [P_Aix;P_Aiy]=M*[h1;h2;...;h8]
# 这样我们可以取求h1--h8了
# 所以现在的问题 首先变成了 怎么找M矩阵 以及找到P_A矩阵的四个交点(为了与B对应)



from PIL import Image
import numpy as np
import matplotlib.pyplot as plt
from grid_corresponding import grid_corresponding
from interp2_bilinear import interp2_bilinear
import cv2

# 第一步肯定是再如图片  显示是为了手动选取四个角点，记录下来
im = np.array(Image.open('./image1.jpg')).astype(float)/255.

f1 = plt.figure()
ax1 = f1.add_subplot(121)
ax1.imshow(im)

 
#coordonnées des coins de la feuille dans l'image
#ordre : haut gauche, haut droit, bas droit, bas gauche
p_A = np.array([[305.,831.,3671.,4293.],[2238.,244.,211.,2219.]])  # 注意尺寸 x;y 也就是说第一个点坐标为 (305,2238)

# 这段纯属好看 没用
markersize = 30
ax1.scatter(p_A[0,0],p_A[1,0],ec='r', s=markersize, fc='none')
ax1.scatter(p_A[0,1],p_A[1,1],ec='g', s=markersize, fc='none')
ax1.scatter(p_A[0,2],p_A[1,2],ec='b', s=markersize, fc='none')
ax1.scatter(p_A[0,3],p_A[1,3],ec='k', s=markersize, fc='none')

ax1.set(title='Image prise')


# 我们现在构建一个A4纸 大小是题目说的 21cm par 29,7cm 也就是宽度是21cm 高度29.7cm
#coordonnées des coins de la feuille idéale format A4
#ordre : haut gauche, haut droit, bas droit, bas gauche
scale = 100
h_rec = int(29.7*scale)
w_rec = int(21.*scale)
p_B = np.array([[0., w_rec-1, w_rec-1, 0.],[0., 0., h_rec-1, h_rec-1]]) # 四个角

ax2 = f1.add_subplot(122)
ax2.scatter(p_B[0,0],p_B[1,0],ec='r', s=markersize, fc='none')
ax2.scatter(p_B[0,1],p_B[1,1],ec='g', s=markersize, fc='none')
ax2.scatter(p_B[0,2],p_B[1,2],ec='b', s=markersize, fc='none')
ax2.scatter(p_B[0,3],p_B[1,3],ec='k', s=markersize, fc='none')

ax2.set(xlim = [0, w_rec-1],
       ylim = [0, h_rec-1],
       title='Document rectifié')
ax2.invert_yaxis()

# 现在P_B P_A坐标都有了 开始着手求H ===>利用M矩阵(维度8*8)

def DLT(p_A, p_B):
    p_A_vec = p_A.ravel(order='F')  # 不做过多要求 将二维数组转换为一维数组 按列优先顺序展平
    M = np.zeros((8,8))
    
    for i in range(4):
        pBx = p_B[0,i]
        pBy = p_B[1,i]
        pAx = p_A[0,i]
        pAy = p_A[1,i]
        
        M_line_1 = [pBx, 0, -pBx*pAx, pBy, 0, -pBy*pAx, 1, 0]
        M_line_2 = [0, pBx, -pBx*pAy, 0, pBy, -pBy*pAy, 0, 1]
        
        M[i*2:(i+1)*2,:] = np.array([M_line_1, M_line_2])
    
    
    
    h = np.linalg.solve(M.T.dot(M), M.T.dot(p_A_vec))
    
    H_AB = np.array([[h[0], h[3], h[6]], [h[1], h[4], h[7]], [h[2], h[5], 1]])
    return H_AB

H_AB = DLT(p_A, p_B)
# 我们利用自定义的P_A P_B对应坐标点 得到了对应像	素图像之间点H矩阵 

temp = H_AB.dot(np.vstack([p_B, np.ones((1,4))]))

#  np.vstack([p_B, np.ones((1,4))])       这一步的作用是将 p_B 从 2x4 矩阵扩展为 3x4 的齐次坐标表示 


p_A_pred_x = temp[0,:]/temp[2,:]
p_A_pred_y = temp[1,:]/temp[2,:]

ax1.scatter(p_A_pred_x[0],p_A_pred_y[0],fc='r', s=markersize, marker='x')
ax1.scatter(p_A_pred_x[1],p_A_pred_y[1],fc='g', s=markersize, marker='x')
ax1.scatter(p_A_pred_x[2],p_A_pred_y[2],fc='b', s=markersize, marker='x')
ax1.scatter(p_A_pred_x[3],p_A_pred_y[3],fc='k', s=markersize, marker='x')

#%% Apply transformation
# 已经得到H了，接下来我们要找的是 从理想相机图像B(A4纸)变换到相机图像A(不好的图像)的时候，之间的对应关系
# 这里非常像TP1中的网格对应关系，只是直接从一个相机图像变换到另一个相机图像 不需要K k 之类的参数 为什么？ 因为有H 靠的是H参数

XI,YI = grid_corresponding(H_AB, h_rec, w_rec)

I_undist,_ = interp2_bilinear(im, XI, YI) #将图片进行双线性插值，一插值就能得到修改后的正常图像了
ax2.imshow(I_undist)

import numpy as np

def grid_corresponding(H_AB, h_ideal, w_ideal):
    """
    该函数生成用于应用同基变换的插值网格，基于同基矩阵 H_AB 

    :param H_AB: 同基矩阵 (3x3)
    :param h_ideal: B图像高度 (以像素为单位)
    :param w_ideal: B图像宽度 (以像素为单位)
    :return: XI, YI - 对应图像的插值网络坐标
    """

    # 为校正后的图像（理想图像）创建坐标网格
    x = np.arange(0, w_ideal)    
    y = np.arange(0, h_ideal)
    X, Y = np.meshgrid(x, y)  # 校正图像的坐标网格

    # 创建理想图像的齐次点矩阵
    p_ideal = np.ones((h_ideal, w_ideal, 3))  # (h_ideal x w_ideal x 3)
    p_ideal[:, :, 0] = X  # x 坐标
    p_ideal[:, :, 1] = Y  # y 坐标
    
    # 应用同基变换，得到原始图像中的对应点
    p_image_reel = p_ideal @ (H_AB.T)  # (h_ideal x w_ideal x 3)
    
    # 归一化，恢复齐次坐标
    XI = p_image_reel[:, :, 0] / p_image_reel[:, :, 2]  # 原始图像中的 x 坐标
    YI = p_image_reel[:, :, 1] / p_image_reel[:, :, 2]  # 原始图像中的 y 坐标
    
    return XI, YI

#%% Comparison opencv

H_AB_cv2, mask = cv2.findHomography(p_B.astype(np.float32).T, p_A.astype(np.float32).T)
print(np.abs(H_AB-H_AB_cv2).sum())


# # 使用OpenCV计算的单应性矩阵对图像进行变换
# XI_cv2, YI_cv2 = grid_corresponding(H_AB_cv2, h_rec, w_rec)

# # 使用DLT和OpenCV得到的单应性矩阵分别对图像进行插值
# I_undist_dlt, _ = interp2_bilinear(im, XI, YI)
# I_undist_cv2, _ = interp2_bilinear(im, XI_cv2, YI_cv2)

# # 计算两个结果图像之间的差异
# image_difference = np.abs(I_undist_dlt - I_undist_cv2)

# # 显示差异图像
# plt.figure()
# plt.imshow(image_difference, cmap='gray')
# plt.title('Difference Image between DLT and OpenCV Homography')
# plt.colorbar()
# plt.show()

from PIL import Image
import numpy as np
import cv2 as cv
import matplotlib.pyplot as plt

I_A = np.array(Image.open('images/01.jpg'))
I_B = np.array(Image.open('images/03.jpg'))

I_A_float = I_A.astype(float)/255.
I_B_float = I_B.astype(float)/255.

I_A_grey = ((I_A_float.sum(axis=2)/3)*255).astype(np.uint8)
I_B_grey = ((I_B_float.sum(axis=2)/3)*255).astype(np.uint8)

# 初始化SIFT检测器
sift = cv.SIFT_create()
# 使用SIFT查找关键点和描述符
kpA, desA = sift.detectAndCompute(I_A_grey,None)
pts_A = cv.KeyPoint_convert(kpA)
kpB, desB = sift.detectAndCompute(I_B_grey,None)
# 使用默认参数的BFMatcher
bf = cv.BFMatcher()
matches = bf.knnMatch(desA,desB,k=2)
# 应用比例测试
good = []
for m,n in matches:
    if m.distance < 0.75*n.distance:
        good.append([m])
# cv.drawMatchesKnn 需要以列表形式传递匹配信息
fig1, ax1 = plt.subplots(1)
ax1.imshow(cv.drawMatchesKnn(I_A,kpA,I_B,kpB,good,None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS))
plt.pause(0.01)

def DLT(p_A, p_B):
    p_A_vec = p_A.ravel(order='F')
    M = np.zeros((8,8))
    
    for i in range(4):
        pBx = p_B[0,i]
        pBy = p_B[1,i]
        pAx = p_A[0,i]
        pAy = p_A[1,i]
        
        M_line_1 = [pBx, 0, -pBx*pAx, pBy, 0, -pBy*pAx, 1, 0]
        M_line_2 = [0, pBx, -pBx*pAy, 0, pBy, -pBy*pAy, 0, 1]
        
        M[i*2:(i+1)*2,:] = np.array([M_line_1, M_line_2])
    
    h = np.linalg.solve(M.T.dot(M), M.T.dot(p_A_vec))
    
    H_AB = np.array([[h[0], h[3], h[6]], [h[1], h[4], h[7]], [h[2], h[5], 1]])
    return H_AB
  
nIt = 500
thresh = 1.5 #pix

nInliers_best = 0
for it in range (nIt):
    
    #Sample 4 matches
    rng = np.random.default_rng()
    ids = rng.choice(nMatches, size=4, replace=False)
    
    p_A_sel = p_A[ids,:]
    p_B_sel = p_B[ids,:]
    
    #Estimate homography
    H_AB = DLT(p_A_sel.T, p_B_sel.T)
    #H_AB, mask = cv.findHomography(p_B_sel, p_A_sel)

    #Compute cost
    p_A_pred = (temp := p_B_hom.dot(H_AB.T)) / temp[:, 2:3]
    mask_inliers = ((p_A_hom - p_A_pred)**2).sum(axis=1) < thresh**2
    nInliers = mask_inliers.sum()
    
    if(nInliers_best<nInliers):
        nInliers_best = nInliers
        H_AB_best = H_AB
        mask_inliers_best = mask_inliers
        print('Iteration {} : {} inliers'.format(it,nInliers_best))

inlier_matches = [good[idx] for idx in np.nonzero(mask_inliers_best)[0]]
fig2, ax2 = plt.subplots(1)
ax2.imshow(cv.drawMatchesKnn(I_A,kpA,I_B,kpB,inlier_matches,None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS))
plt.pause(0.01)

# 计算边界
hA, wA, _ = I_A.shape
corners_IA = np.array([[0, wA, wA, 0], [0, 0, hA, hA], [1, 1, 1, 1]]).T
hB, wB, _ = I_A.shape
corners_IB = np.array([[0, wB, wB, 0], [0, 0, hB, hB], [1, 1, 1, 1]]).T
corners_IB_in_IA = (temp := corners_IB.dot(H_AB_best.T)) / temp[:, 2:3]

minx = min([min(corners_IA[:,0]),min(corners_IB_in_IA[:,0])])
maxx = max([max(corners_IA[:,0]),max(corners_IB_in_IA[:,0])])
miny = min([min(corners_IA[:,1]),min(corners_IB_in_IA[:,1])])
maxy = max([max(corners_IA[:,1]),max(corners_IB_in_IA[:,1])])

fig4, axs4 = plt.subplots(1)
axs4.plot(corners_IA[:,0],corners_IA[:,1],'x',label='corners $I_A$')
axs4.plot(corners_IB_in_IA[:,0],corners_IB_in_IA[:,1],'or',label='corners $I_B$ in $I_A$')
axs4.plot([minx, maxx, maxx, minx, minx],[miny, miny, maxy, maxy, miny],label='mosaic bounds')
axs4.grid(True)
axs4.invert_yaxis()
axs4.legend()

w_mosaic = int(maxx - minx + 1)
h_mosaic = int(maxy - miny + 1)

def getInterpolationGrids(Hij, h_j, w_j):

    x = np.arange(0,w_j)
    y = np.arange(0,h_j)
    X, Y = np.meshgrid(x,y) #h_j x w_j
    
    p_j = np.ones((h_j,w_j,3)) #h_j x w_j x 3
    p_j[:,:,0] = X
    p_j[:,:,1] = Y
        
    p_i = (temp := p_j @ (Hij.T)) / temp[:,:,2:3] #h_j x w_j x 3


    XI = p_i[:,:,0] #h_j x w_j
    YI = p_i[:,:,1] #h_j x w_j
 

    return XI, YI

H_MA = np.array([[1., 0., -minx], [0., 1., -miny], [0, 0, 1]]) #the origin of I_A has coordinates -minx,-miny in the mosaic
H_AM = np.array([[1., 0., minx], [0., 1., miny], [0, 0, 1]])
XI_A, YI_A = getInterpolationGrids(H_AM, h_mosaic, w_mosaic)
I_mosA = cv.remap(I_A_float.astype(np.float32), XI_A.astype(np.float32), YI_A.astype(np.float32), cv.INTER_LINEAR) #very fast but inaccurate

fig5, axs5 = plt.subplots(2,2)
axs5[0,0].imshow(XI_A)
axs5[0,1].imshow(YI_A)

H_BM = np.linalg.inv(H_AB_best) @ H_AM
XI_B, YI_B = getInterpolationGrids(H_BM, h_mosaic, w_mosaic)
I_mosB = cv.remap(I_B_float.astype(np.float32), XI_B.astype(np.float32), YI_B.astype(np.float32), cv.INTER_LINEAR) #very fast but inaccurate

axs5[1,0].imshow(XI_B)
axs5[1,1].imshow(YI_B)

fig6, axs6 = plt.subplots(1,3)
axs6[0].imshow(I_mosA)
axs6[1].imshow(I_mosB)
axs6[2].imshow(0.5*(I_mosA + I_mosB))

Image.fromarray((0.5*(I_mosA + I_mosB)*255).astype(np.uint8)).save("mosaic_2_im.png")

# 我们要实现两个图片的拼接，具体思路为  使用 随机抽样一致性（Random Sample Consensus，RANSAC）算法 可实现图像的自动对应 换句话说 本实验也是和单应性相关的 
# 算法步骤如下 
# 进行循环 每次循环从所有的符合对应点中选取四个点(一般是四个 因为最低是四个) 
# 利用对应的循环点来估计H矩阵，就是我们之前实现中的DLT算法 输入 P_B 和P_A 输出H_BA 
# 但是现在有一个问题 就是我们不确定对应点是否正确，也就是说得到的H并不一定是正确的，怎么办
# 在每次循环中，我们使用得到的H 来和P_B相乘得到估计P_est_A  再将估计值和实际值做欧几里得距离 如果距离近 则说明估计的差不多 是好H 是对应点 否则 不用
# 因此评判当前矩阵H是好是坏的标准是 利用阈值得到的正确对应点个数 ， 越大就说明H越优


# 注意本实验需要先找到对应点 利用已知算法SIFT 代码由老师提供 不与研究

from PIL import Image
import numpy as np
import cv2 as cv
import matplotlib.pyplot as plt
# 由此可见，这几个库函数是必须要背的，第一个就是numpy，第二个就是cv2 as cv，第三个是matplotlib.pyplot as plt，第四个是如果要导入图像，必须用的from PIL import image


# 第一步 把图像导入进来 ---一分
I_A=np.array(Image.open('01.jpg'))
I_B=np.array(Image.open('02.jpg'))

# 第二步 导入图像要动手，要动手就得用float函数来转换

I_A_float = I_A.astype(float)/255. #只要植入图像 要对图像动手 必须先转换成float
I_B_float = I_B.astype(float)/255.

I_A_grey = ((I_A_float.sum(axis=2)/3)*255).astype(np.uint8)
I_B_grey = ((I_B_float.sum(axis=2)/3)*255).astype(np.uint8)

# Initiate SIFT detector
sift = cv.SIFT_create()
# find the keypoints and descriptors with SIFT
kpA, desA = sift.detectAndCompute(I_A_grey,None)
pts_A = cv.KeyPoint_convert(kpA)
kpB, desB = sift.detectAndCompute(I_B_grey,None)
# BFMatcher with default params
bf = cv.BFMatcher()
matches = bf.knnMatch(desA,desB,k=2)
# Apply ratio test
good = []
for m,n in matches:
 if m.distance < 0.75*n.distance:
     good.append([m])
# cv.drawMatchesKnn expects list of lists as matches.
fig1, ax1 = plt.subplots(1)
ax1.imshow(cv.drawMatchesKnn(I_A,kpA,I_B,kpB,good,None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS))
plt.pause(0.01)
nMatches = len(good)

# 上述部分不研究
p_A = np.float32([kpA[m[0].queryIdx].pt for m in good])
p_A_hom = np.ones((nMatches,3))
p_A_hom[:,:2] = p_A
p_B = np.float32([kpB[m[0].trainIdx].pt for m in good])
p_B_hom = np.ones((nMatches,3))
p_B_hom[:,:2] = p_B
# 最后我们得到了对应点P_A P_B之间的坐标， 并且用齐次坐标来表示P_A_hom P_B_hom，接下来，就要开始RANSAC算法来求H_AB了


thresh = 1.5 
inlier_best = 0
# 首先定义循环 
for i as range(500)
# 我们要从所有的对应点中找到四个对应点
ids=random(4) ===>     ids = np.random.choice(n_matches, 4, replace=False)

P_A_select = P_A(ids,:)
P_B_select = P_B(ids,:)

# 接下来我们用对应点来寻找H矩阵 利用DLT算法

H_AB = DLT(P_A_select,P_B_select)

# 得到了H_AB继续估计P_A_est    

P_A_hom_est = H_AB*P_B_hom    ===>    p_B_hom_transformed = (H_AB @ p_B_hom.T).T  # Shape: (n_matches, 3)

P_A_est = P_A_hom_est/P_A_hom_est[:,2]  ===>    p_B_transformed = p_B_hom_transformed[:, :2] / p_B_hom_transformed[:, 2][:, np.newaxis]


distance = sqrt(sum((P_A_est-P_A)**2))   ===>    distances = np.linalg.norm(p_B_transformed - p_A, axis=1)

if distance < thresh 
	inlier = 

if inlier_best< inlier
	inlier_best = inlier
	H_best = H_AB
	print(inlier_best,H_AB_best)
	
# 经过上述代码，我们就可以找到最优的H_AB
# 接下来我们着手拼接图像，需要构建一个新的大小，并保证足够大来包含拼接后的所有图像
# 所以首先我们必须获取两个图像的边界 也就是将两个图像四个角点坐标给出
hA,wA,_ = I_A.shape;
corners_IA = np.array([[0,wA,wA,0],[0,0,hA,hA],[1,1,1,1]])

hB,wB,_ = I_B.shape;
corners_IB = np.array([[0,wB,wB,0],[0,0,hB,hB],[1,1,1,1]])

# 但是注意 千万不要直接用corners_IB这个，因为这是两个图像屏幕的像素坐标，我们要把B变到A平面的像素坐标
corners_IB_in_IA = 

# 找到角干什么用？我们把它们做对比，找到最边的四个角，来当作新图像的四个角，这样图像大小就肯定适合了

# 拼接图像的范围
minx = min(min[corners_IA(:,0) , corners_IB(:,0)] )
miny = min(min[corners_IA(:,1) , corners_IB(:,1)] )
maxx = max(max[corners_IA(:,0) , corners_IB(:,0)] )
maxx = max(max[corners_IA(:,0) , corners_IB(:,0)] )


# 然后我们定义新图像的宽和高为
w_mosaic = int(maxx-minx + 1)
h_mosaic = int(maxy-miny + 1)

# 我们将两个图片依次映射到新的图像坐标中--- TP2 


# 也就是我们已经有了H_MA的情况下，得到最终的新图像 方法 === 网格插值
# 这里要定义H_AM但是我不知道为什么

XI_A,YI_A = grid_corresponding(H_AM,h_mosaic,w_mosaic)
I_imageA_in_M= = cv.remap(I_A_float.astype(np.float32), XI_A.astype(np.float32), YI_A.astype(np.float32), cv.INTER_LINEAR) #very fast but inaccurate


H_BM = np.linalg.inv(H_AB_best) @ H_AM

XI_B, YI_B = getInterpolationGrids(H_BM, h_mosaic, w_mosaic)
I_mosB = cv.remap(I_B_float.astype(np.float32), XI_B.astype(np.float32), YI_B.astype(np.float32), cv.INTER_LINEAR)

from PIL import Image
import numpy as np
import cv2 as cv
import matplotlib.pyplot as plt

# Step 1: Load the images
image_filenames = [f'{i:02d}.jpg' for i in range(1, 11)]
images = [np.array(Image.open(fname)) for fname in image_filenames]

# Initialize the panorama with the first image
panorama = images[0]

# Step 8: Define the function to compute homography using DLT
def compute_homography(p_src, p_dst):
    """
    Compute homography H such that p_dst ~ H * p_src
    p_src and p_dst are arrays of shape (n, 2)
    """
    n = p_src.shape[0]
    if n < 4:
        raise ValueError("At least four points are required to compute homography")
    A = []
    for i in range(n):
        x_src, y_src = p_src[i, :]
        x_dst, y_dst = p_dst[i, :]
        A.append([-x_src, -y_src, -1, 0, 0, 0, x_src * x_dst, y_src * x_dst, x_dst])
        A.append([0, 0, 0, -x_src, -y_src, -1, x_src * y_dst, y_src * y_dst, y_dst])
    A = np.array(A)
    # Solve A * h = 0 using SVD
    U, S, Vh = np.linalg.svd(A)
    h = Vh[-1, :]  # The last row of Vh corresponds to the smallest singular value
    H = h.reshape(3, 3)
    return H

def stitch_images(I_A, I_B):
    # Step 2: Convert images to float and grayscale
    I_A_float = I_A.astype(float) / 255.
    I_B_float = I_B.astype(float) / 255.

    I_A_gray = ((I_A_float.sum(axis=2) / 3) * 255).astype(np.uint8)
    I_B_gray = ((I_B_float.sum(axis=2) / 3) * 255).astype(np.uint8)

    # Step 3: Detect SIFT keypoints and descriptors
    sift = cv.SIFT_create()
    kpA, desA = sift.detectAndCompute(I_A_gray, None)
    kpB, desB = sift.detectAndCompute(I_B_gray, None)

    # Step 4: Match descriptors using BFMatcher and apply ratio test
    bf = cv.BFMatcher()
    matches = bf.knnMatch(desA, desB, k=2)

    good_matches = []
    for m, n in matches:
        if m.distance < 0.75 * n.distance:
            good_matches.append(m)

    # Step 5: Extract matching points
    p_A = np.float32([kpA[m.queryIdx].pt for m in good_matches])
    p_B = np.float32([kpB[m.trainIdx].pt for m in good_matches])

    # Step 6: Convert points to homogeneous coordinates
    n_matches = len(good_matches)
    if n_matches < 4:
        print("匹配点不足，无法拼接 ")
        return I_A  # 返回原始图像
    p_A_hom = np.hstack((p_A, np.ones((n_matches, 1))))
    p_B_hom = np.hstack((p_B, np.ones((n_matches, 1))))

    # Step 7: Define RANSAC parameters
    thresh = 1.5
    inlier_best = 0
    n_iterations = 500

    # Step 9: Implement the RANSAC algorithm
    for i in range(n_iterations):
        # Randomly select 4 matching points
        ids = np.random.choice(n_matches, 4, replace=False)
        p_A_sel = p_A[ids, :]
        p_B_sel = p_B[ids, :]

        # Estimate homography using the selected points
        H_AB = compute_homography(p_B_sel, p_A_sel)

        # Transform all points from image B to image A coordinate system
        p_B_hom_transformed = (H_AB @ p_B_hom.T).T  # Shape: (n_matches, 3)
        p_B_transformed = p_B_hom_transformed[:, :2] / p_B_hom_transformed[:, 2][:, np.newaxis]

        # Compute Euclidean distances between the transformed points and the actual points in image A
        distances = np.linalg.norm(p_B_transformed - p_A, axis=1)

        # Identify inliers where the distance is less than the threshold
        inliers = distances < thresh
        n_inliers = np.sum(inliers)

        # Update the best homography if the current one has more inliers
        if n_inliers > inlier_best:
            inlier_best = n_inliers
            H_best = H_AB
            inliers_best = inliers
            # print(f'Iteration {i}: {inlier_best} inliers')

    # Step 11: Warp images using the best homography
    # Get image dimensions
    h_A, w_A = I_A.shape[:2]
    h_B, w_B = I_B.shape[:2]

    # Get the corners of image B
    corners_B = np.array([[0, 0], [w_B, 0], [w_B, h_B], [0, h_B]], dtype=np.float32)
    corners_B_hom = np.hstack((corners_B, np.ones((4, 1))))

    # Warp the corners of image B to image A coordinate system
    corners_B_transformed_hom = (H_best @ corners_B_hom.T).T
    corners_B_transformed = corners_B_transformed_hom[:, :2] / corners_B_transformed_hom[:, 2][:, np.newaxis]

    # Combine corners from both images to get the size of the panorama
    corners_A = np.array([[0, 0], [w_A, 0], [w_A, h_A], [0, h_A]], dtype=np.float32)
    all_corners = np.vstack((corners_A, corners_B_transformed))

    # Find the bounding box of the panorama
    [x_min, y_min] = np.int32(all_corners.min(axis=0) - 0.5)
    [x_max, y_max] = np.int32(all_corners.max(axis=0) + 0.5)

    # Compute the translation needed
    translation = [-x_min, -y_min]

    # Compute the size of the panorama
    panorama_size = (x_max - x_min, y_max - y_min)

    # Adjust the homography to include the translation
    H_translation = np.array([[1, 0, translation[0]],
                              [0, 1, translation[1]],
                              [0, 0, 1]])

    # Warp image B into the panorama
    I_B_warped = cv.warpPerspective(I_B, H_translation @ H_best, panorama_size)

    # Create the panorama canvas
    panorama = np.zeros((panorama_size[1], panorama_size[0], 3), dtype=np.uint8)
    panorama[translation[1]:h_A + translation[1], translation[0]:w_A + translation[0]] = I_A

    # Blend the warped image B into the panorama
    mask = (I_B_warped > 0)
    panorama[mask] = I_B_warped[mask]

    return panorama

# Step 4: Stitch images in a loop
for i in range(1, len(images)):
    print(f'正在拼接第 {i+1} 张图片...')
    I_B = images[i]
    panorama = stitch_images(panorama, I_B)

# Step 12: Display the final stitched image
plt.figure(figsize=(20, 15))
plt.imshow(panorama)
plt.title('Résultat de la fusion panoramique')
plt.axis('off')
plt.show()

from PIL import Image
import numpy as np
import cv2 as cv
import matplotlib.pyplot as plt

#%% LOAD IMAGES
I_A = np.array(Image.open('data/1.png'))
I_B = np.array(Image.open('data/3.png'))

I_A_float = I_A.astype(float)/255.
I_B_float = I_B.astype(float)/255.

fig1, axs1 = plt.subplots(1,2)
axs1[0].imshow(I_A);
axs1[1].imshow(I_B);

# 可见这部分代码是必备必背的，其思路顺序为
# - 必要库函数的导入
# - 导入图像并转为float，同时显示一下原图

# 后续代码由老师提供，不管
#%% SIFT & MATCHING
I_A_grey = ((I_A_float.sum(axis=2)/3)*255).astype(np.uint8)
I_B_grey = ((I_B_float.sum(axis=2)/3)*255).astype(np.uint8)

# Initiate SIFT detector
sift = cv.SIFT_create()
# find the keypoints and descriptors with SIFT
kpA, desA = sift.detectAndCompute(I_A_grey,None)
pts_A = cv.KeyPoint_convert(kpA)
kpB, desB = sift.detectAndCompute(I_B_grey,None)
# BFMatcher with default params
bf = cv.BFMatcher()
matches = bf.knnMatch(desA,desB,k=2)
# Apply ratio test
good = []
for m,n in matches:
 if m.distance < 0.75*n.distance:
     good.append([m])
# cv.drawMatchesKnn expects list of lists as matches.
fig1, ax1 = plt.subplots(1)
ax1.imshow(cv.drawMatchesKnn(I_A,kpA,I_B,kpB,good,None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS))
plt.pause(0.01)

nMatches = len(good)
p_A = np.float32([kpA[m[0].queryIdx].pt for m in good])
p_A_hom = np.ones((nMatches,3))
p_A_hom[:,:2] = p_A
p_B = np.float32([kpB[m[0].trainIdx].pt for m in good])
p_B_hom = np.ones((nMatches,3))
p_B_hom[:,:2] = p_B

（F_AB, _ = cv.findFundamentalMat(p_B_sel, p_A_sel, method=cv.FM_8POINT)）# 这部分是在RANSACF代码中体现的，不是直接用在这里
#推荐的 ransacF 函数原型如下：
F_AB, mask_inliers, nInliers = ransacF(p_A_hom, p_B_hom, nItRansac, threshRansac)。

import numpy as np
import cv2 as cv

def ransacF(p_A, p_B, nIt, thresh):  # p_A 形状为 (nMatches, 3)

    nMatches = p_A.shape[0] # 获取匹配点对的数量 
     # p_A 大小为 (nMatches, 3)，也就是(匹配点对的数量，3)      3 是齐次坐标
     # p_A.shape[0] 获取矩阵的第一维度的大小，即行数，这里代表匹配点对的数量
     # 如果 p_A 的形状是 (500, 3)，那么 nMatches 的值就是 500
    nInliers_best = 0
    for it in range(nIt):
        
        #Sample 8 matches
        rng = np.random.default_rng()
        ids = rng.choice(nMatches, size=8, replace=False)

        p_A_sel = p_A[ids,:] # 
        p_B_sel = p_B[ids,:]
        # 这里是标准的: 已知匹配对，从中随机选x个 的代码
        # 因此我们得到了，带有八个随机匹配对的(P_A,P_B)坐标 称之为select 用于估计F_AB
        # 随机选择一组最小样本数据（在这种情况下是8对匹配点）来拟合模型，并使用该模型评估所有数据点的内点数量
        
        #Estimate fondamental matrix
        #F_AB = eight_pt(p_A_sel.T, p_B_sel.T)
        F_AB, _ = cv.findFundamentalMat(p_B_sel, p_A_sel, method=cv.FM_8POINT)
    
        if(F_AB is not None):
            if(np.isnan(F_AB).sum() == 0): 
            # sum() 会计算布尔数组中 True 的个数，即矩阵中 NaN 值的数量
            # 如果这个数量为 0，说明矩阵中没有任何 NaN 值，则认为矩阵有效
                
                #Compute cost   计算每个匹配点对的代价（cost）
                num = np.array([np.abs(p_A[m,:]@F_AB@(p_B[m,:].T)) for m in range(nMatches)])
                denum = np.array([np.sqrt(((F_AB[:2,:]@(p_B[m,:].T))**2).sum()) for m in range(nMatches)])
                dist = num/denum
                mask_inliers = dist < thresh
                nInliers = mask_inliers.sum()
                
               #至此我们计算了对应距离，下面是标准的最优判断，如果距离足够小则被判断为内点，并保存最优的参数
              
              
                if(nInliers_best<nInliers):
                    nInliers_best = nInliers
                    F_AB_best = F_AB
                    mask_inliers_best = mask_inliers
                    print('{} inliers'.format(nInliers_best))# 随着迭代的进行，我们可以找到越来越大的内点数
    
    
    F_AB_best, _ = cv.findFundamentalMat(p_B[mask_inliers_best,:], p_A[mask_inliers_best,:], method=cv.FM_8POINT)
    # 这一步我认为不需要添加
    return F_AB_best, mask_inliers_best, nInliers_best
        

#%% 以下代码用于测试
from gui_F import gui_F
from ransacF import ransacF

#%% RANSAC & FUNDAMENTAL MATRIX ESTIMATION
nItRansac = 500
threshRansac = 1.5 #in pixels

F_AB, mask_inliers, nInliers = ransacF(p_A_hom,p_B_hom,nItRansac,threshRansac)

inlier_matches = [good[idx] for idx in np.nonzero(mask_inliers)[0]]
fig2, ax2 = plt.subplots(1)
ax2.imshow(cv.drawMatchesKnn(I_A,kpA,I_B,kpB,inlier_matches,None,flags=cv.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS))
plt.pause(0.01)

p_A_inliers = p_A_hom[mask_inliers,:]
p_B_inliers = p_B_hom[mask_inliers,:]

from gui_F import gui_F
gui_F(Image.fromarray(I_A), Image.fromarray(I_B), F_AB)

 - **如果新的代价函数值降低了**（说明更新有效），则减小$\lambda$，使算法更接近高斯-牛顿法，加快收敛 

 - **如果代价函数值没有降低**， 则增大$\lambda$，使算法更接近梯度下降法，保证稳定性