基于单目视觉的平面目标定位和坐标测量 (上) - 坐标系和成像模型

起因

在一个项目的实验过程中，需要对遥控小车的位置进行跟踪和测算。在一穷二白只有一个空房间的情况下，只能自己动手造工具了。本着开放共享的精神，以及为挽救同胞们的头发着想，有必要把原理和过程写下来。所以本文将叙述：如何使用五毛钱成本搭建一个基于视觉的目标测量平台。

 

目的和环境

人为指定空房间的坐标系，例如左下角是原点，向上是z轴；房间地板上有一个待测坐标的目标；房间地面上放置若干位置已知的定位点；任意放置摄像机，拍摄包含了所有这些元素的图像；编写程序计算目标的位置。

下面的示意图表达了上面这段画的意思。

流程

 

1.    预备知识

1.1 三种坐标系

相机坐标系(Xc, Yc, Zc)：以相机感光原件为坐标系原点，Zc轴与镜头的光轴平行。所有相机坐标系中的坐标点是以摄像机的视角而言的。对于空间中固定的一点，其在相机坐标系中的坐标会随着相机位置的变动而改变。

世界坐标系(Xw, Yw, Zw)：人为定义的、实际空间中的坐标系。例如在本例中，人为指定了房间左下角为世界坐标系原点，向上是Z轴。世界坐标系与相机坐标系本没有任何关系，但物理空间中某点的坐标可以在两个坐标系之间转换。转换的方法也很简单，就是旋转变换+平移变换：

世界坐标系和相机坐标系

像平面坐标系(u, v)：成像之后照片中的坐标系，二维平面，单位是像素，坐标原点已经不在中心，而是在画面左上角。如图所示：

 

通过相机的成像模型，可以将相机坐标系中的三维点映射到成像屏幕上的二维点。

1.2 成像模型

初中的物理课上学到过，凸透镜满足下列公式，称为”薄镜公式”
 

$$\frac{1}{f} = \frac{1}{{do}} + \frac{1}{{di}}$$

在相机镜头上，do>>di≈f
可以简化得到针孔相机成像模型：
 
这里把成像面移到了右边，但不妨碍数学上的正确。根据相似三角形原理，有：
 

因此，相机坐标系中的点[X,Y,Z]转换到二维成像面上的[x,y]，可以通过下列公式实现：

这里，[X,Y,Z]和[x,y]的量纲仍旧一样，是物理长度；[x,y]的坐标原点还是成像平面与光轴的交点。

但是，在图像处理时，处理对象是像素，而且图像左上角才是原点，如下图。

所以从相机坐标系到图像坐标的变换如下。

其中3*4矩阵的第4列将原点移到了左上角；fx的量纲不再是长度，而是像素/长度，实际上，fx=f/px, px是x方向上像素的宽度。
此外，这个3*3矩阵也叫做相机的内参矩阵，因为传感器和相机镜头的参数决定了这个矩阵。

再把世界坐标系也引入进来，最终的成像模型表达式如下(!划重点!)

其中A由相机本身决定，称之为相机矩阵或内参(intrinsic parameters)矩阵，R为旋转矩阵，T为平移向量，称之为外部参数。

2. 相机标定

相机标定的目的是为了求得内参矩阵A和镜头畸变系数dist (distortion coefficients).
内参矩阵A的含义在前面已经叙述过了，畸变系数dist是为了矫正镜头对成像造成的畸变。畸变包括径向畸变(radial distortion)和切向畸变(tangential distortion)。
最典型的径向畸变是广角镜头的成像效果；而切向畸变来源于镜头和感光器件组装时的不平行，在五块钱的相机模组上尤其严重。

下图展示了径向畸变和切向畸变的效果。
 

径向畸变

 

切向畸变

 

径向畸变和切向畸变可以通过数学模型尽可能地复原，但也仅是近似复原而已，再也回不到最初的模样了。畸变矫正的模型如下：
 
其中k1-k6用于矫正径向畸变，称为径向畸变系数。p1、p2用于矫正切向畸变系数。
根据矫正时所用阶数的不同，opencv中使用的畸变参数格式如下，
 
参数的数量可以是4、5、8.

相机标定的本质是：对已知角点三维位置关系的标志板成像，使图像点的预定义位置（根据三维点的投影计算得到）和实际位置（图像中的位置）之间的距离达到最小。每个点在标定过程中都会产生这个距离，它们的累加和就是重投影误差. 以重投影误差为优化目标，求解内存矩阵A和畸变参数dist. 求解过程有点复杂，但前人的成果已经变成函数可以用了。张氏标定算法（Zhang. A Flexible New Technique for Camera Calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000）就是做这件事的。

标志点一般使用棋盘图案
当然也可以用圆盘
有闲情逸志自己在地上画几个点也可以，
理论上只要能得到一组[ 物理世界三维坐标 - 图像二维像素坐标]的集合就可以。
所以你可以在这里找到各种可用于标定的pattern. 

本文中使用棋盘标志板，手机摄像头拍摄。现在手机的镜头都有自动对焦功能，不同的焦点下内参是不一样的，因此需要用手动模式固定对焦点到无穷远。推荐一款软件cinema FV-5 可以在摄像时锁定对焦点。

然后从不同角度，不同距离，不同位置拍摄二十张左右的棋盘图片

Opencv中相机标定的核心函数是calibrateCamera()。我们准备使用棋盘标志板，用python语言，函数的输出输出如下：

cv2.calibrateCamera(objectPoints, imagePoints, imageSize[, cameraMatrix[, distCoeffs[, rvecs[, tvecs[, flags[, criteria]]]]]]) → retval, cameraMatrix, distCoeffs, rvecs, tvecs

调用calibrateCamera()前需要先准备好feed它的参数，主要是标志板上的三维坐标点集合objectPoints和对应的二维像素坐标imagePoints.

objectPoints是已知的，棋盘上的角点位置是四四方方的排布，直接构造就好了，
 

objp = np.zeros((cornerX*cornerY,3), np.float32)
objp[:,:2] = np.mgrid[0:cornerX,0:cornerY].T.reshape(-1,2) * squareSize

求解内存A和畸变系数时，棋盘格子的长度单位其实不需要知道，只需要知道相对位置即可，因为长度比例并不影响这两个参数的求解。棋盘格子的长度最终会影响外参R和T. 但每个此从棋盘图像求解到的外参并不一样，我们暂且也不需要这些外参。所以上面的 squareSize其实可以直接设成1，opencv的例子中直接就没有乘以 squareSize这个过程，如果不知道标定的原理，看了就容易让人产生困惑。

通过检测图片中的角点确定对应的图像二维坐标点，这里使用findChessboardCorners()函数.

运行程序，标定结束之后在终端显示内参矩阵和畸变系数：

$ python calibrate.py 

('mtx: \n', array([[  1.58908007e+03,   0.00000000e+00,   9.63684080e+02],
       [  0.00000000e+00,   1.59532586e+03,   5.61602291e+02],
       [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00]]))
('dist: \n', array([[  4.17032261e-01,  -1.97266626e+00,   2.26847015e-03,
          1.37087506e-03,   3.12510368e+00]]))
calibration finished, result saved as .npz file 

完整的程序如下。

#!/usr/bin/ python
# -*- coding: utf-8 -*- 
#從文件夾讀取棋盤圖片,校準相機,將參數保存到文件.
#2019年7月17日
#guofeng， mailto:gf@gfshen.cn
#---------------------------------------
import numpy as np
import cv2
import glob

#ref: https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_calib3d/py_calibration/py_calibration.html



def  calibrate(cornerX=8,cornerY=6, squareSize=24.5,images=glob.glob('*.jpg')  ):
    # termination criteria
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((cornerX*cornerY,3), np.float32)
    objp[:,:2] = np.mgrid[0:cornerX,0:cornerY].T.reshape(-1,2) * squareSize  #如果squareSize设错，对mtx和dist的估计没有影响，但对rvecs和tvecs有影响

    # Arrays to store object points and image points from all the images.
    objpoints = [] # 3d point in real world space
    imgpoints = [] # 2d points in image plane.

   

    for fname in images:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

        # Find the chess board corners
        ret, corners = cv2.findChessboardCorners(gray, (cornerX,cornerY),None)

        # If found, add object points, image points (after refining them)
        if ret == True:
            objpoints.append(objp)

            corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
            imgpoints.append(corners2)

            # Draw and display the corners
            img = cv2.drawChessboardCorners(img, (cornerX,cornerY), corners2,ret)
            cv2.imshow('img',img)
            cv2.waitKey(500)

    cv2.destroyAllWindows()
    #开始标定
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

    if ret:
        print('mtx: \n',mtx)
        print('dist: \n', dist)
        #print('rvecs: \n', rvecs)
        #print('tvecs: \n',tvecs)
        np.savez('calibrateData.npz', mtx=mtx, dist=dist)

        
    
    
    
    return ret, mtx, dist, rvecs, tvecs




def undistortion(img, mtx, dist):
    h, w = img.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))

    dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

    # crop the image
    x, y, w, h = roi
    if roi != (0, 0, 0, 0):
        dst = dst[y:y + h, x:x + w]

    return dst


if __name__ == '__main__':

    image=glob.glob('./chessBoardNote3/*.bmp')
    if not image:
        raise RuntimeError('Cant find image,exting')
        
    #用于存储内参矩阵和畸变参数
    mtx = []
    dist = []
    try:
        npzfile = np.load('calibrate.npz')
        mtx = npzfile['mtx']
        dist = npzfile['dist']
    except IOError:
        ret, mtx, dist, rvecs, tvecs = calibrate( cornerX=8,cornerY=6, squareSize=24.5,images= image  )
    print('calibration finished, result saved as .npz file ')

至此，得到了相机内参。接下来将使用已知参数的相机，保持固定焦点，拍摄包含目标的图像，然后测算目标位置。