计算机视觉——基本矩阵的计算

最近在上研究生的课程《计算机视觉》，完成了老师布置的大作业，结合我看《计算机视觉中的多视图几何》的一些感悟和收获完成此篇博客。在学习的过程中我发现很多算法并没有开源，或者版本太落后难以执行，因此想通过这篇博客将一些算法展现出来，让更多的人在学习的过程中少走弯路！笔者水平能力有限，如有错误，敬请指出。

基本矩阵

关于基本矩阵的介绍，网上很多博客都写的很好，这里就不再赘述了。基本矩阵主要是应用在两视图几何中的，用来约束两视图间的匹配点对，因此下面我们实现的代码都是基于两视图的。

特征点匹配+GMS过滤外点

首先我们需要提取出两个视图中的匹配点对。这里采用了效果比较好的SIFT算法和外点过滤效果好的GMS算法得到匹配点对。

1. 读取图片，计算SIFT特征点
2. 暴力匹配得到尽可能多的匹配对
3. GMS算法对BFMatch结果进行过滤

def SIFT(img):
    sift = cv2.xfeatures2d.SIFT_create(100)
    
    # 使用SIFT检测关键点  
    kps,decs = sift.detectAndCompute(img, None)  
    return kps,decs
    
left_img = cv2.imread("./left2.jpg")
right_img = cv2.imread("./right2.jpg")
kps1,desc1 = SIFT(left_img)
kps2,desc2 = SIFT(right_img)
matcher  = cv2.BFMatcher_create()
matches_all = matcher.match(desc1, desc2)
matches_gms = matchGMS(left_img.shape[:2], right_img.shape[:2], kps1, kps2, matches_all, withScale=True,
                    withRotation=True, thresholdFactor=6)
match_result = cv2.drawMatches(left_img, kps1, right_img, kps2, matches_gms,None,flags=2)

我们可以展示匹配结果看一下匹配效果：

这里SIFT点我只提取了100个，因为当我把匹配点设置过多的时候，内点率会逐渐下降。

计算基本矩阵

opencv已经给出了一个函数库cv2.findFundamentalMat()，但这不利于提高我们的编码能力和对算法的理解程度，因此我们手动实现归一化八点算法，同时库函数和手动计算得到的结果进行对比。
库函数
首先给出库函数计算过程和结果：

src_points = np.float32([kps1[m.queryIdx].pt for m in matches_gms]).reshape(-1, 2)
dst_points = np.float32([kps2[m.trainIdx].pt for m in matches_gms]).reshape(-1, 2)
F,mask = cv2.findFundamentalMat(src_points,dst_points,cv2.FM_RANSAC)
F = [-5.77184137e-07 -6.67998326e-06  6.56611343e-03]
    [ 7.18639182e-06 -4.60223599e-07 -4.59609635e-03]
    [-6.47510627e-03  3.81943240e-03  1.00000000e+00]

归一化八点算法
可能有些同学还不清楚步骤，这里我简述一下具体步骤：

1. 先对点坐标系进行平移，使所有点位于几何中心。然后使所有点距离坐标中心的平均距离为$\sqrt{2}$
2. SVD求解方程组得到$\hat{F}$
3. $\hat{F}$一般是秩为3，而rank(F)=2，因此需要通过SVD调整$\hat{F}$的秩
4. 调整回原坐标系下的F，即进行步骤1的逆过程

第一步进行数据归一化，关键是平移矩阵和缩放矩阵的构建，在计算的过程中可以多关注一下矩阵的shape。

src_ax,src_ay = np.mean(src_points,axis=0)
dst_ax,dst_ay = np.mean(dst_points,axis=0)
n = len(src_points)
## 平移矩阵
T1 = np.array([[1,0,-src_ax],
               [0,1,-src_ay],
               [0,0,1]])
T2 = np.array([[1,0,-dst_ax],
               [0,1,-dst_ay],
               [0,0,1]])
p1 = (T1 @ np.array([[x,y,1] for x,y in src_points]).T).T
p2 = (T2 @ np.array([[x,y,1] for x,y in dst_points]).T).T
src_sum = 0
dst_sum = 0
for i in range(n):
    src_sum += math.sqrt(p1[i][0]**2+p1[i][1]**2)
    dst_sum += math.sqrt(p2[i][0]**2+p2[i][1]**2)
src_sum /= n
dst_sum /= n
alpha = math.sqrt(2) / src_sum
beta = math.sqrt(2) / dst_sum
## 缩放矩阵
S1 = np.array([[alpha,0,0],
               [0,alpha,0],
               [0,0,1]])
S2 = np.array([[beta,0,0],
               [0,beta,0],
               [0,0,1]])
u = (S1 @ p1.T).T
v = (S2 @ p2.T).T

第二步构建方程组，利用SVD求解超定方程组。
$$\left[\begin{array}{ccc}x& y& 1\end{array}\right]\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=0$$
把上述表达展开，并以$[f_{ij}]$为未知向量构建$AX=0$的方程：
$$\begin{gathered} x^{\prime }x{f}_{11}+x^{\prime }y{f}_{12}+x^{\prime }{f}_{13}+y^{\prime }x{f}_{21}+y^{\prime }y{f}_{22}+y^{\prime }{f}_{23}+x{f}_{31}+y{f}_{32}+f_{33}=0 \\ [x^{\prime }x,x^{\prime }y,x^{\prime },y^{\prime }x,y^{\prime }y,y^{\prime },x,y,1]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ f_{21}\\ f_{22}\\ f_{23}\\ f_{31}\\ f_{32}\\ f_{33}\end{array}\right]=0 \end{gathered}$$
因为我们已知$[x,y,1{]}^T$和$[x^{\prime },y^{\prime },1{]}^T$，每一组匹配点对便可以构建出一个方程。F的自由度为7，那么至少需要八对点来求解F。通常我们得到的匹配点对超过了8对，对于超定方程组我们需要利用多余的方程计算最小二乘解。

最小二乘法就是通过拟合实际数据得到的函数使得通过函数得到的数据与实际数据之间误差的平方和为最小

我们对A矩阵进行SVD分解，$U,\Sigma ,V^T=SVD(A)$，我们所求的最小二乘解就是矩阵V的最后一列。

这是因为矩阵V最后一列对应$\Sigma$中的特征值是最小的，所以误差也就最小

上述求得基本矩阵$\hat{F}$的秩通常为3，而实际$rank(F)=2$，因此我们需要对$\hat{F}$再进行SVD分解，并将$\Sigma$中最后一列的元素设置为0，再将$U$、$\Sigma$、$V^T$乘回去得到$F$。

l1 = np.multiply(v[:,0],u[:,0])
l2 = np.multiply(v[:,0],u[:,1])
l3 = v[:,0]
l4 = np.multiply(v[:,1],u[:,0])
l5 = np.multiply(v[:,1],v[:,1])
l6 = v[:,1]
l7 = u[:,0]
l8 = u[:,1]
l9 = np.ones_like(v[:,1])
A = np.vstack((l1,l2,l3,l4,l5,l6,l7,l8,l9)).T
U,Sigma,VT = np.linalg.svd(A)
V = VT.T[:,-1].reshape(-1,3)
U,D,VT = np.linalg.svd(V)
D[2] = 0
F_ = U @ np.diag(D) @ VT
# eight_F = (S1 @ T1).T @ F_ @ S2 @ T2
eight_F_tmp = (S2 @ T2).T @F_ @(S1 @ T1)
eight_F = eight_F_tmp / eight_F_tmp[2,2]
F = [-6.01698176e-07 -8.10162516e-06  7.81740018e-03]
    [ 8.47767046e-06 -6.76200912e-07 -5.09137869e-03]
    [-7.60729655e-03  4.50740583e-03  1.00000000e+00]

我们计算一下两种方法计算F的相似程度，采用了余弦相似度计算两矩阵中每一个对应位置元素的相似程度。

def cosine_similarity(matrix1, matrix2):
    dot_product = np.dot(matrix1, matrix2.T)
    norm_matrix1 = np.linalg.norm(matrix1, axis=1)
    norm_matrix2 = np.linalg.norm(matrix2, axis=1)
    similarity = abs(dot_product) / np.outer(norm_matrix1, norm_matrix2)
    return similarity
    
 simi = [1.         0.99999809 0.99995647]
        [0.99999825 0.99999999 0.99997203]
        [0.99996774 0.99998164 0.99999912]

以上就是归一化八点法的全部内容了。涉及到的理论部分并不多，主要想分享一下具体实现。之后大致还会安排一下几个内容，先挖个坑：

1. 相机标定中的黄金标准算法
2. 通过极线和极点构建投影矩阵
3. 张正友标定法