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 = []  # 2d points in image plane.
    images = glob.glob('../images/boards/left*.jpg')
    images += glob.glob('../images/boards/right*.jpg')
    for fname in images:#遍历所有图像
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)#变灰度
        # 找到棋盘角点
        ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
        #如果找到，添加对象点、图像点
        if ret == True:
            objpoints.append(objp)
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)#亚像素精确化角点
            imgpoints.append(corners)
            #画出角点
            cv2.drawChessboardCorners(img, (7, 6), corners2, ret)
            cv2.imshow('img', img)
            cv2.waitKey(500)
    cv2.destroyAllWindows()

4.摄像机标定、校正图像、计算误差

• 我们使用函数cv2.calibrateCamera()来标定。它会返回摄像机矩阵，畸变系数，旋转和变换向量等。
• 我们可以使用从函数cv2.getOptimalNewCameraMatrix() 得到的自由缩放系数对摄像机矩阵进行优化。如果缩放系数alpha = 0，返回的非畸变图像会带有最少量的不想要的像素。它甚至有可能在图像角点去除一些像素。如果alpha = 1，所有的像素都会被返回，还有一些黑图像。它还会返回一个ROI 图像，我们可以用来对结果进行裁剪。
• 我们找到一幅图像来进行校正。OpenCV 提供了两种方法。
• 最后我们可以利用反向投影误差对我们找到的参数的准确性进行估计。得到的结果越接近0 越好。有了内部参数，畸变参数和旋转变换矩阵，我们就可以使用cv2.projectPoints() 将对象点转换到图像点。然后就可以计算变换得到图像与角点检测算法的绝对差了。然后我们计算所有标定图像的误差平均值。

代码速记：

• cv2.calibrateCamera()
• cv2.getOptimalNewCameraMatrix()
• cv2.undistort()
• cv2.initUndistortRectifyMap()
• cv2.remap()
• cv2.projectPoints()
• cv2.norm()

部分参数解释：

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.objpoints, self.imgpoints, gray.shape[::-1], None, None)
#retval, 摄像机矩阵, 畸变系数, rvecs, tvecs =calibrateCamera(对象点, 图像点, 图像大小, 摄像机矩阵, 畸变系数,rvecs,tvecs,             flags,criteria)

newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
#retval, validPixROI= 摄像机矩阵、畸变系数、图像大小、alpha、新图像大小、                          centerPrincipalPoint

dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
#结果图像 = 原图像、摄像机矩阵、畸变系数、结果图像，优化的摄像机矩阵(默认None)

实战：

def calib(self):
    # 【1】读入一张图片进行准备进行校正
    img = cv2.imread('../images/boards/left12.jpg')
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)  # 变灰度

    # 【2】进行摄像机标定，得到摄像机矩阵、畸变系数等
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.objpoints, self.imgpoints, gray.shape[::-1], None, None)

    # 【3】优化摄像机矩阵
    h, w = img.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))#返回自由缩放系数alpha和ROI图像
    #alpha = 0，返回的非畸变图像会带有最少量的不想要的像素
    #alpha = 1，所有的像素都会被返回，还有一些黑图像
    #ROI 图像，我们可以用来对结果进行裁剪

    # 【4】校正图像：两种方法结果相同
    # 方法一：undistort函数
    dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
    x, y, w, h = roi
    dst = dst[y:y + h, x:x + w]# 结合上边得到的ROI 对结果进行裁剪
    # 方法二：remapping：
    # mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)#首先我们要找到从畸变图像到非畸变图像的映射方程。
    # dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)#再使用重映射方程。
    # x, y, w, h = roi
    # dst = dst[y:y + h, x:x + w]

    # 【5】画出结果
    titles = ['raw', 'dst']
    imgs = [img, dst]
    for i in range(2):
        plt.subplot(1, 2, i + 1), plt.imshow(cv2.cvtColor(imgs[i], cv2.COLOR_BGR2RGB))
        plt.title(titles[i])
        plt.xticks([]), plt.yticks([])
    plt.show()

    #【6】计算误差
    mean_error = 0
    for i in range(len(self.objpoints)):
        imgpoints2, _ = cv2.projectPoints(self.objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(self.imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
        mean_error += error
    print("total error: ", mean_error / len(self.objpoints))

total error: 0.02395964795252433

5.姿势估计

• 在上一节的摄像机标定中，我们已经得到了摄像机矩阵，畸变系数等。有了这些信息我们就可以估计图像中图案的姿势，比如目标对象是如何摆放，如何旋转等。
• 对一个平面对象来说，我们可以假设Z=0，这样问题就转化成摄像机在空间中是如何摆放（然后拍摄）的。
• 如果我们知道对象在空间中的姿势，我们就可以在图像中绘制一些2D 的线条来产生3D 的效果。
• 我们的问题是，在棋盘的第一个角点绘制3D 坐标轴（X，Y，Z 轴）。X轴为蓝色，Y 轴为绿色，Z 轴为红色。在视觉效果上来看，Z 轴应该是垂直与棋盘平面的。

代码速记：

• np.mgrid()
• cv2.findChessboardCorners()
• cv2.cornerSubPix()
• cv2.solvePnP()
• cv2.projectPoints()

实战：

绘图函数

def draw(img, corners, imgpts):#参数：输入图像、棋盘上的角点、要绘制的3D 坐标轴上的点
    corner = tuple(corners[0].ravel())
    #画出x,y,z轴
    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255, 0, 0), 5)
    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5)
    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5)
    return img
    
def draw_cube(img,imgpts):
    imgpts = np.int32(imgpts).reshape(-1, 2)
    #用绿色画底面
    img = cv2.drawContours(img, [imgpts[:4]], -1, (0, 255, 0), -3)
    #用蓝色画柱子
    for i, j in zip(range(4), range(4, 8)):
        img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3)
    #用红色画顶面
    img = cv2.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3)
    return img

姿势估计

def gestrue(self,mtx, dist):
    #【1】加载前面结果中摄像机矩阵和畸变系数(本函数使用参数传递)
    #【2】设置终止条件、对象点(棋盘上的3D角点)和坐标轴点
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    objp = np.zeros((6 * 7, 3), np.float32)#初始化对象点为0
    objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)#mgrid函数返回多维结构

    axis1 = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)
    #X 轴从（0,0,0）绘制到（3,0,0），Y 轴从（0,0,0）绘制到（0,3,0）
    #Z 轴从（0,0,0）绘制到（0,0,-3）。负值表示它是朝着（垂直于）摄像机方向。

    #3D 空间中的一个立方体的8 个角点
    axis2 = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0],
                       [0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]])

    #【3】加载图像，检测角点，计算变换矩阵并投影
    img = cv2.imread('../images/boards/left12.jpg')
    copy1=img.copy()
    copy2=img.copy()
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)#检测角点
    if ret == True:#检测到角点
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)#亚像素精确化
        ret, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)#计算旋转和平移变换矩阵
        #【4】利用变换矩阵把坐标轴点投影到平面，并调用绘图函数
        
        #3个坐标轴
        imgpts1, jac = cv2.projectPoints(axis1, rvecs, tvecs, mtx, dist)
        draw(copy1, corners2, imgpts1)
        #立方体
        imgpts2, jac = cv2.projectPoints(axis2, rvecs, tvecs, mtx, dist)
        draw_cube(copy2,imgpts2)
        
        #【5】展示图像
        titles = ['raw', 'dst_axis','dst_cube']
        imgs = [img, copy1,copy2]
        for i in range(3):
            plt.subplot(1, 3, i + 1), plt.imshow(cv2.cvtColor(imgs[i], cv2.COLOR_BGR2RGB))
            plt.title(titles[i])
            plt.xticks([]), plt.yticks([])
        plt.show()

每条坐标轴的长度都是3 个格子的长度。