基于OpenCV的位姿估计

今天我们的目标是找出我们相对于球场上的位置，从而了解我们在比赛中的全局位置。

01.什么是单应性

单应性是一种平面关系，可将点从一个平面转换为另一个平面。它是一个3乘3的矩阵，转换3维矢量表示平面上的2D点。这些向量称为同质坐标，下面将进行讨论。下图说明了这种关系。这四个点在红色平面和图像平面之间相对应。单应性存储相机的位置和方向，这可以通过分解单应性矩阵来检索。

针孔相机模型是相机的数学表示。它接受3D点并将其投影到像上图所示的图像平面上。该模型的重要方面是焦点，像平面（上图中的灰度平面），主点（上图中的像面上的粗体点），焦距（像平面与像之间的距离）焦点）和光轴（垂直于穿过焦点的像平面的线）。可以在投影矩阵中编码该变换，该投影矩阵将表示3D点的4维均匀向量转换为表示图像平面上2d点的3维均匀向量。

齐次坐标是表示计算机视觉中的点的投影坐标。由于拍摄照片时会从3D转换为2D，因此深度范围会丢失。因此，可以将无限数量的3D点投影到相同的2D点，这使得同质坐标在描述可能性射线时非常通用，因为它们的比例相似。齐次坐标仅取直角坐标，并将维数增大到末端。

用齐次坐标表示的笛卡尔坐标，在比例上也相等。

请注意，三角形可能会越来越远且更大，但仍然可以产生相同的图像给定同质坐标，将所有元素除以矢量的最后一个元素（比例因子），然后笛卡尔坐标是一个矢量，该矢量由除最后一个元素之外的所有元素组成。

02.投影矩阵

投影矩阵是与相机属性相关的其他两个矩阵的乘积。它们是外部和内部相机矩阵。这些矩阵分别存储摄像机的外部参数和固有参数（因此命名）。

投影矩阵（3 x 4矩阵）

外参矩阵

外在矩阵存储摄像机在全局空间中的位置。该信息存储在旋转矩阵以及平移矢量中。旋转矩阵存储相机的3D方向，而平移矢量将其位置存储在3D空间中。

旋转矩阵

然后将旋转矩阵和平移向量连接起来以创建外部矩阵。从功能上讲，外部矩阵将3D同类坐标从全局坐标系转换为相机坐标系。因此，所有变换后的矢量将相对于焦点在空间中表示相同的位置。

内参矩阵

本征矩阵存储相机的本征，例如焦距和主点。焦距（f 1和f 6）是从焦点到像面的距离，可以用像素宽度或像素高度（因此为何有2个焦距）来度量。每个像素都不是一个完美的正方形，因此每个边都有不同的边长。主点（c和c）是光轴和像平面（像平面的功能中心）的交点。该矩阵将相对于焦点的3D坐标转换到图像平面上；将其视为拍摄照片的矩阵。当与外部矩阵组合时，将创建针孔相机模型。

针孔相机数学模型

现在，单应性是针孔相机模型的特殊情况，其中投影到相机上的所有现实世界坐标都位于z坐标为0的平面上。

H是单应性矩阵，是3 x 3矩阵，可将点从一个平面转换为另一个平面。在这里，变换是在Z = 0的平面和指向该点的图像平面之间进行的投影。单应性矩阵通常通过4点算法求解。本质上，它使用来自2个平面的4个点对应来求解单应矩阵。在OpenCV中，我们可以使用cv2.findHomography方法找到单应矩阵：

cv2.findHomography(, )

此方法需要某种形式的特征点跟踪，以便上面方法的结果。坐标测量的质量将有助于上述方法的准确性。一旦有了单应性矩阵，就可以将其分解为摄像机的平移和旋转。单应矩阵的分解如下所示：

我们可以通过将解决方案矩阵的前两列用作旋转矩阵中的前两列，然后使用叉积来找到旋转矩阵的最后一列，从而得出旋转矩阵。翻译是解决方案矩阵的最后一列。

03.分解代码

'''
H is the homography matrix
K is the camera calibration matrix
T is translation
R is rotation
'''
H = H.T
h1 = H[0]
h2 = H[1]
h3 = H[2]
K_inv = np.linalg.inv(K)
L = 1 / np.linalg.norm(np.dot(K_inv, h1))
r1 = L * np.dot(K_inv, h1)
r2 = L * np.dot(K_inv, h2)
r3 = np.cross(r1, r2)
T = L * (K_inv @ h3.reshape(3, 1))
R = np.array([[r1], [r2], [r3]])
R = np.reshape(R, (3, 3))

代码链接：
https://github.com/RaubCamaioni/OpenCV_Position

04，优势

使用Homography比其他算法简单得多，因为它非常简单直观。利用基本或基本矩阵的其他方法需要复杂的算法和更多的实现精力。由于所有视觉本地化方法都在做相同的事情，因此最好在可能的情况下使用Homography，以节省时间和精力。

（小白学视觉独家授权头条号）