opencv摄像机标定+姿势估计

1.原理

• 今天的低价单孔摄像机（照相机）会给图像带来很多畸变。畸变主要有两种：径向畸变和切想畸变。如果我们想对畸变的图像进行校正就必须找到五个造成畸变的系数：Distortion cofficients = $(k1;k2;p1;p2;k3)$。
• 除此之外，我们还需要再找到一些信息，比如摄像机的内部和外部参数。外部参数与旋转和变换向量相对应，它可以将3D点的坐标转换到坐标系统中。内部参数是摄像机特异的。它包括的信息有焦距$（fx,fy）$，光学中心$（cx,cy）$等。这也被称为摄像机矩阵。它完全取决于摄像机自身，只需要计算一次，以后就可以已知使用了。可以用下面的3x3 的矩阵表示：
  
• 在3D 相关应用中，必须要先校正这些畸变。为了找到这些参数，我们必须要提供一些包含明显图案模式的样本图片（比如说棋盘）。我们可以在上面找到一些特殊点（如棋盘的四个角点）。我们知道它在现实世界中的坐标，也知道它在图像中的坐标。有了这些信息，我们就可以使用数学方法求解畸变系数。 为了得到更好的结果，我们至少需要10 个这样的图案模式。

2.如何找到对象点和图像点

（1）测量对象点

我们要输入一组3D 真实世界中的点以及与它们对应2D 图像中的点。2D 图像的点可以在图像中很容易的找到。那么真实世界中的3D 的点呢？这些图像来源与静态摄像机和棋盘不同的摆放位置和朝向。所以我们需要知道（X，Y，Z）的值。但是为了简单，我们可以说棋盘在XY 平面是静止的，（所以Z 总是等于0）摄像机在围着棋盘移动。这种假设让我们只需要知道X，Y 的值就可以了。现在为了求X，Y 的值，我们只需要传入这些点（0,0），（1,0），（2,0）…，它们代表了点的位置。在这个例子中，我们的结果的单位就是棋盘（单个）方块的大小。但是如果我们知道单个方块的大小（假如说30mm），我们输入的值就可以是（0,0），（30,0），（60,0）…，结果的单位就是mm。3D 点被称为对象点，2D 图像点被称为图像点。

（2）找到图像点：角点

• 为了找到棋盘的图案，我们要使用函数cv2.findChessboardCorners()。我们还需要传入图案的类型，比如说8x8 的格子或5x5 的格子等。在本例中我们使用的7x6 的格子。（通常情况下棋盘都是8x8 或者7x7）。它会返
  回角点，如果得到图像的话返回值类型（Retval）就会是True。这些角点会按顺序排列（从左到右，从上到下）。
• 这个函数可能不会找出所有图像中应有的图案。所以一个好的方法是编写代码，启动摄像机并在每一帧中检查是否有应有的图案。在我们获得图案之后我们要找到角点并把它们保存成一个列表。在读取下一帧图像之前要设置一定的间隔，这样我们就有足够的时间调整棋盘的方向。继续这个过程直到我们得到足够多好的图案。
• 除了使用棋盘之外， 我们还可以使用环形格子， 但是要使用函数cv2.findCirclesGrid() 来找图案。据说使用环形格子只需要很少的图像就可以了。

（3）找到亚像素角点

在找到这些角点之后我们可以使用函数cv2.cornerSubPix() 增加准确度。我们使用函数cv2.drawChessboardCorners() 绘制图案。

3.用opencv方法获取对象点和图像点

代码速记：

• glob.glob()
• cv2.findChessboardCorners()
• cv2.cornerSubPix()
• cv2.drawChessboardCorners()

参数解释：

#获取指定目录下的所有图片
images=glob.glob('../images/boards/left*.jpg')
#只有一个参数pathname，定义了文件路径匹配规则，这里可以是绝对路径，也可以是相对路径。
#””匹配0个或多个字符；”?”匹配单个字符；”[]”匹配指定范围内的字符

ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)#image、patternSize、corners、flags。
#如果得到图像的话返回值类型（Retval）就会是True

cv2.drawChessboardCorners(img, (7, 6), corners2, ret)#image、patternSize、corners、patternWasFound

实战：

def find_corners(self):
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)#终止条件
    # 对象点：like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((6 * 7, 3), np.float32)
    objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
    objpoints = []  # 3d point in real world space
    imgpoints = [