opencv双目视觉标定，激光结构光提取，指定特征点获取世界坐标

双目视觉标定，激光结构光提取，指定特征点获取世界坐标

标定方面
校正
结构光提取
二维点转换为三维点
总结

这学期在做双目视觉方面的事情，因为没人带，自己一个人踩了很多坑，因此在这写一点自己的总结心得。

标定方面

首先标定主要就是为了确定左右相机的内参和两个相机之间的旋转跟平移矩阵，这些参数都是后面需要用到的，而网上关于标定的文章太多了，本人试过opencv的标定，也试过matlab的标定方法，最后感觉还是matlab的比较准确，主要步骤可以参考下面这篇文章，写的挺详细了：
双目标定

校正

在校正这方面我主要采用的是opencv中的Bouguet算法，其用处简单来说就是使你的两个相机成为光轴平行的模型，如下图分别为校正前和校正后。

并且校正后会使得图片的极线对齐，所谓极线对齐，简单理解就是对齐后的左右图片的特征点会在同一行像素行上，如下图：

可以看到校正后左边图片上的一点跟右图上对应的一点在同一行上。这一步方便后续的立体匹配也就是特征点的匹配，首先我们得知道立体视觉中最重要的部分就是寻找特征点，所有做的一切都是为了特征点的匹配而做的，试想你在左图上确定一点初始点，要在右图上找到其对应的点，那么就只要沿着初始点的行横向搜索匹配就可以，或者在这一行的附近几行搜索，大大减少了计算量。

结构光提取

因为笔者做的实验是为了计算一个物体的角度，因此用激光照射来辅助提取特征点，并且我只需要获取三个点的世界坐标就能测出角度。如下图，黑点就是我所需要的特征点：

因此我要对激光进行骨架提取，首先进行图片预处理，主要步骤就是先把左右图转换为灰度图，然后进行图片校正获取校正后的左右图片，再进行高斯滤波，然后阈值分解，所得到的左右图效果图如下：

最后就使劲结构光的提取。
在这一步我试过很多方法，也踩过很多坑，比如steger方法，灰度重心法，极值法，方向模板法，脊线跟踪法等，其中steger和脊线主要都是应用海塞矩阵来选择光电，但是笔者在实际中的实验效果却不太理想，不知道是海塞矩阵的问题还是激光的问题，最后根据实际情况选择了方向模板法，python代码如下：

def direction(img):K1 = np.array([[0,0,1,1,1,0,0],[0, 0, 1, 1, 1, 0, 0],[0, 0, 1, 1, 1, 0, 0],[0, 0, 1, 1, 1, 0, 0],[0, 0, 1, 1, 1, 0, 0]])K2 = np.array([[0,0,0,0,0,0,0],[0, 1, 1, 1, 1, 1, 0],[0, 1, 1, 1, 1, 1, 0],[0, 1, 1, 1, 1, 1, 0],[0, 0, 0, 0, 0, 0, 0]])K3 = np.array([[1, 1, 1, 0, 0, 0, 0],[0, 1, 1, 1, 0, 0, 0],[0, 0, 1, 1, 1, 0, 0],[0, 0, 0, 1, 1, 1, 0],[0, 0, 0, 0, 1, 1, 1]])K4 = np.fliplr(K3)M1 = cv.filter2D(img, cv.CV_32FC1, K1)M2 = cv.filter2D(img, cv.CV_32FC1, K2)M3 = cv.filter2D(img, cv.CV_32FC1, K3)M4 = cv.filter2D(img, cv.CV_32FC1, K4)maxindex = 0maxvalue = 0x = 0y = 0Pt = []col = img.shape[1]row = img.shape[0]for i in range(col):flag = Falsefor j in range(row):if img[j, i] > 200:maxvalue = max(M1[j,i],M2[j,i],M3[j,i],M4[j,i])if(maxvalue>=maxindex):x = iy = jflag = Trueif(flag):Pt.append(x)Pt.append(y)new_img = np.zeros(img.shape, dtype=np.uint8)for k in range((int(len(Pt) / 2))):new_img[Pt[2 * k + 1], Pt[2 * k]] = 255return new_img

方向模板法主要原理简单讲就是根据几个预先制定的模板对每一行或者每一列进行匹配，然后将值最大的那个点做为主光点，详细的可以去看相关论文，另外还有自适应方向模板法，具体原理其实都差不多。
提取完后的效果图如下：

可以看到总体效果还是可以的。
最后我觉得这一步的前提还是在阈值分割，只有将感兴趣的区域选择出来，对后续的工作才能有所帮助。

二维点转换为三维点

二维点的获取就是特征点的获取，因为的需求很简单，只需要左右两个点和拐点就行，因此提取很简单，就不发出来了，主要说一下特征点是如何获得三维点的。
简单说就是根据前面的标定获得Q矩阵，即重投影矩阵，再根据重投影矩阵反求坐标

def getAngle(A1,B1,C1,A2,B2,C2):Q = c_config.Qprint(Q)cx = Q[0][3]cy = Q[1][3]print("cx = ", cx)print("cy =", cy)f = Q[2][3]E = Q[3][3]Tx = Q[3][2]# Tx = 0.008333333# Tx = 0.01667print(f)print(Tx)print(E)print("1=",A1,B1,C1)print("2=", A2, B2, C2)deep = np.array([A1[1]-A2[1],B1[1]-B2[1] , C1[1]-C2[1]])print("deep=",deep)w = deep * Tx + EZ = f / wy = np.array([A1[0], B1[0], C1[0]])x = np.array([A1[1], B1[1], C1[1]])X = (x + cx)/wY = (y + cy)/wT = np.vstack((X, Y, Z)).Tprint("T= ", T)print(T[0])print(T[1])print(T[2])A = np.array([T[0][0], T[0][1], T[0][2]])B = np.array([T[1][0], T[1][1], T[1][2]])C = np.array([T[2][0], T[2][1], T[2][2]])BA = A-BBC = C-Bprint(BA, BC)print(A,B,C)cosangle = BA.dot(BC) / (np.linalg.norm(BA) * np.linalg.norm(BC))# cosangle = cos_dist(BA,BC)Lba = np.sum(np.linalg.norm(BA))Lbc = np.sum(np.linalg.norm(BC))print("BA.BC=",BA.dot(BC))print("Lba=", Lba)print("Lbc=", Lbc)print("cos=", cosangle)angle = np.arccos(cosangle)print(np.degrees(angle))ax = plt.subplot(111, projection='3d')  # 创建一个三维的绘图工程#  将数据点分成三部分画，在颜色上有区分度ax.scatter(T[0][0], T[0][1], T[0][2], c='y')  # 绘制数据点ax.scatter(T[1][0], T[1][1], T[1][2], c='r')ax.scatter(T[2][0], T[2][1], T[2][2], c='g')ax.plot([A[0], B[0], C[0]], [A[1], B[1], C[1]], [A[2], B[2], C[2]], label='parametric curve')ax.set_zlabel('Z')  # 坐标轴ax.set_ylabel('Y')ax.set_xlabel('X')plt.show()

重投影矩阵Q实现了世界坐标系{world}和像素坐标系{pixel}之间的转换。具体如下：

这里Q矩阵中的参数都是左相机的参数，除了Cx‘以外，但一般校正正确后，Cx = Cx’，所以Q矩阵的最后一项为0，然后用左图片的特征点跟视差求得三维坐标，并且这里默认世界坐标系是跟左相机坐标系重合的，如下：所示

其中d = Xl - Xr，Xl为左图片点像素坐标，Xr为右图片点像素坐标。
至此，就能获得特征点三维坐标。

总结

对越角度计算来说，最主要的还是特征点的提取，只有特征点提取正确，才能获取正确的世界坐标,而提取在特征点的前提就是要有较好的结构光骨架，因此对结构光的骨架提取要求很高。其实总的来说就是一点点小东西，但因为都是一个人在做，所以在实验的时候还是绕了很多弯路，踩了很多坑，因此写此文以简单总结一下个人心得，希望能帮助到一点进来看本文的你。