SLAM入门之视觉里程计(6)：相机标定张正友经典标定法详解

https://www.cnblogs.com/wangguchangqing/p/8335131.html

想要从二维图像中获取到场景的三维信息，相机的内参数是必须的，在SLAM中，相机通常是提前标定好的。张正友于1998年在论文："A Flexible New Technique fro Camera Calibration"提出了基于单平面棋盘格的相机标定方法。该方法介于传统的标定方法和自标定方法之间，使用简单实用性强，有以下优点：

不需要额外的器材，一张打印的棋盘格即可。
标定简单，相机和标定板可以任意放置。
标定的精度高。

相机的内参数

设P=(X,Y,Z)P=(X,Y,Z)为场景中的一点，在针孔相机模型中，其要经过以下几个变换，最终变为二维图像上的像点p=(μ,ν)p=(μ,ν):

将PP从世界坐标系通过刚体变换(旋转和平移)变换到相机坐标系，这个变换过程使用的是相机间的相对位姿，也就是相机的外参数。
从相机坐标系，通过透视投影变换到相机的成像平面上的像点p=(x,y)p=(x,y)。
将像点pp从成像坐标系，通过缩放和平移变换到像素坐标系上点p=(μ,ν)p=(μ,ν)。

相机将场景中的三维点变换为图像中的二维点，也就是各个坐标系变换的组合，可将上面的变换过程整理为矩阵相乘的形式：

s⎛⎝⎜⎜μν1⎞⎠⎟⎟=⎡⎣⎢⎢⎢α000β0cxcy1⎤⎦⎥⎥⎥⎡⎣⎢⎢f000f0001000⎤⎦⎥⎥[R0Tt1]⎛⎝⎜⎜⎜⎜XYZ1⎞⎠⎟⎟⎟⎟=⎡⎣⎢⎢⎢fx000fy0cxcy1000⎤⎦⎥⎥⎥[R0Tt1]⎛⎝⎜⎜⎜⎜XYZ1⎞⎠⎟⎟⎟⎟s(μν1)=[α0cx0βcy001][f0000f000010][Rt0T1](XYZ1)=[fx0cx00fycy00010][Rt0T1](XYZ1)

将矩阵KK称为相机的内参数,

K=⎡⎣⎢⎢⎢fx000fy0cxcy1⎤⎦⎥⎥⎥K=[fx0cx0fycy001]

其中，α,βα,β表示图像上单位距离上像素的个数，则fx=αf,fy=βffx=αf,fy=βf将相机的焦距ff变换为在x,y方向上像素度量表示。

另外，为了不失一般性，可以在相机的内参矩阵上添加一个扭曲参数γγ，该参数用来表示像素坐标系两个坐标轴的扭曲。则内参数KK变为

K=⎡⎣⎢⎢⎢fx00γfy0cxcy1⎤⎦⎥⎥⎥K=[fxγcx0fycy001]

对于大多数标准相机来说，可将扭曲参数γγ设为0. Multiple View Geometry in Computer Vision

张氏标定法

在上一篇博文单应矩阵，介绍的单应矩阵表示两个平面间的映射。在张氏标定法中，用于标定的棋盘格是三维场景中的一个平面ΠΠ，其在成像平面的像是另一个平面ππ，知道了两个平面的对应点的坐标，就可以求解得到两个平面的单应矩阵HH。其中，标定的棋盘格是特制的，其角点的坐标是已知的；图像中的角点，可以通过角点提取算法得到，这样就可以得到棋盘平面ΠΠ和图像平面ππ的单应矩阵HH。
通过上面的相机模型有：

p=K[R|t]Pp=K[R|t]P

其中pp是像点坐标，PP是标定的棋盘坐标。这样就可以得到下面的等式：

H=K[R|t]H=K[R|t]

HH表示的是成像平面和标定棋盘平面之间的单应矩阵。通过对应的点对解得HH后，则可以通过上面的等式得到相机的内参数KK，以及外参旋转矩阵RR和平移向量tt。

棋盘平面和成像平面间的单应

将一个平面映射到另一个平面，将棋盘格所在的平面映射到相机的成像平面，则有

p=HPp=HP

pp为棋盘格所成像的像点坐标，PP棋盘格角点在世界坐标系的坐标。

设棋盘格所在的平面为世界坐标系中Z=0Z=0的平面，这样棋盘格的任一角点PP的世界坐标为(X,Y,0)(X,Y,0)，根据小孔相机模型：

s⎛⎝⎜⎜uv1⎞⎠⎟⎟=K[Rt]⎛⎝⎜⎜⎜⎜XY01⎞⎠⎟⎟⎟⎟=K[r1r2r3t]⎛⎝⎜⎜⎜⎜XY01⎞⎠⎟⎟⎟⎟=K[r1r2t]⎛⎝⎜⎜XY1⎞⎠⎟⎟s(uv1)=K[Rt](XY01)=K[r1r2r3t](XY01)=K[r1r2t](XY1)

根据平面间的单应性，有

s⎛⎝⎜⎜uv1⎞⎠⎟⎟=H⎛⎝⎜⎜XY1⎞⎠⎟⎟s(uv1)=H(XY1)

将上面两个等式进行整合，则可以得到单应矩阵HH和相机矩阵（包含内参和外参）的相等，如下：

H=λK[r1r2t]H=λK[r1r2t]

这样就可以使用棋盘平面和成像平面间的单应矩阵来约束相机的内参和外参。单应矩阵HH可以通过棋盘平和成像平面上对应的点计算出来。

内参的约束条件

通过平面间的单应，可以得到如下的等式

H=[h1h2h3]=λK[r1r2t]H=[h1h2h3]=λK[r1r2t]

将旋转矩阵RR的各个列向量和平移向量tt使用HH的列向量表示，

r1=λK−1h1r2=λK−1h2t=λK−1h3r1=λK−1h1r2=λK−1h2t=λK−1h3

又由于，RR是旋转矩阵，则其是正交矩阵，也就是其任意两个列向量的内积为0，列向量的模为1。故有：

rT1r2=0‖r1‖=‖r2‖=1r1Tr2=0‖r1‖=‖r2‖=1

则对于一幅棋盘标定版的图像（一个单应矩阵）可以获得两个对内参数的约束等式：

{hT1K−TK−1h2=0hT1K−TK−1h1=hT2K−TK−1h2=1{h1TK−TK−1h2=0h1TK−TK−1h1=h2TK−TK−1h2=1

求解内参数

通过一幅标定板的图像可以的得到关于内参数的两个等式，令

B=K−TK−1=⎡⎣⎢⎢B11B21B31B12B22B32B13B23B33⎤⎦⎥⎥=⎡⎣⎢⎢⎢⎢⎢1α2−γα2βv0γ−u0βα2β−γα2βγ2α2β2+1β2−γ(v0γ−u0β)α2β2−v0β2v0γ−u0βα2β−γ(v0γ−u0β)α2β2−v0β2(v0γ−u0β)2α2β2+v0β2+1⎤⎦⎥⎥⎥⎥⎥B=K−TK−1=[B11B12B13B21B22B23B31B32B33]=[1α2−γα2βv0γ−u0βα2β−γα2βγ2α2β2+1β2−γ(v0γ−u0β)α2β2−v0β2v0γ−u0βα2β−γ(v0γ−u0β)α2β2−v0β2(v0γ−u0β)2α2β2+v0β2+1]

注意，矩阵BB是一个对称矩阵，其未知量只有6个，将6个未知量写为向量的形式

b=[B11,B12,B22,B13,B23,B33]b=[B11,B12,B22,B13,B23,B33]

令hihi为单应矩阵HH的第ii个行向量，则有

hi=[hi1,hi2,hi3]Thi=[hi1,hi2,hi3]T

故：

hiK−TK−1hj=hiBhj=vTijb其中，vij=[hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3]ThiK−TK−1hj=hiBhj=vijTb其中，vij=[hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3]T

有了上边的等式，再来看从一幅标定板图像得到的等式

{hT1K−TK−1h2=0hT1K−TK−1h1=hT2K−TK−1h2=1⇒{vT22b=0v11b=v12b{h1TK−TK−1h2=0h1TK−TK−1h1=h2TK−TK−1h2=1⇒{v22Tb=0v11b=v12b

写成矩阵的形式有：

[vT12v11−v22]b=0[v12Tv11−v22]b=0

上面的一幅标定板图像取得的约束等式，假如有nn幅图像，则

Vb=0Vb=0

其中，VV是一个2n×62n×6的矩阵，bb是一个6维向量，所以

当n≥3n≥3，可以得到bb的唯一解;
当n=2n=2，则可以假设扭曲参数γ=0γ=0作为额外的约束条件
当n=1n=1，则值能计算两个相机的内参数

对于方程Vb=0Vb=0可以使用SVD求得其最小二乘解。对VTVVTV进行SVD分解，其最小特征值对应的特征向量就是Vb=0Vb=0的最小二乘解，从而求得矩阵BB。由于这里得到的BB的估计值是在相差一个常量因子下得到的，所以有：

B=λA−TAB=λA−TA

从而可以得到相机的各个内参数：

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪cx=γcy/β−B13α2/λcy=(B12B13−B11B23)/(B11B22−B212)α=λ/B11‾‾‾‾‾√β=λB11/(B11B22−B212)‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾‾√γ=−B12α2β/λλ=B33−[B213+cy(B12B13−B11B23)]/B11{cx=γcy/β−B13α2/λcy=(B12B13−B11B23)/(B11B22−B122)α=λ/B11β=λB11/(B11B22−B122)γ=−B12α2β/λλ=B33−[B132+cy(B12B13−B11B23)]/B11

最大似然估计

上面使用最小二乘法得到估计得到的解，并没有物理上的实际意义，。为了进一步增加标定结果的可靠性，可以使用最大似然估计(Maximum likelihood estimation)来优化上面估计得到的结果。

假设同一相机从nn个不同的角度的得到了nn幅标定板的图像，每幅图像上有mm个像点。MijMij表示第ii幅图像上第jj个像点对应的标定板上的三维点，则

m̂ (K,Ri,ti,Mij)=K[Rt]Mijm^(K,Ri,ti,Mij)=K[Rt]Mij

m̂ (K,Ri,ti,Mij)m^(K,Ri,ti,Mij)表示MijMij的像点。其中，Ri,tiRi,ti表示第ii幅图像对应相机的旋转矩阵和平移向量，KK是相机的内参数。则像点mijmij的概率密度函数是

f(mij)=12π‾‾‾√e−(m̂ (K,Ri,ti,Mij)−mij)2σ2f(mij)=12πe−(m^(K,Ri,ti,Mij)−mij)2σ2

构造似然函数

L(A,Ri,ti,Mij)=∏i=1,j=1n,mf(mij)=12π‾‾‾√e−∑ni=1∑mj=1(m̂ (K,Ri,ti,Mij)−mij)2σ2L(A,Ri,ti,Mij)=∏i=1,j=1n,mf(mij)=12πe−∑i=1n∑j=1m(m^(K,Ri,ti,Mij)−mij)2σ2

为了能够让LL取得最大值，需要最小化下面的值

∑i=1n∑j=1m‖m̂ (K,Ri,ti,Mij)−mij‖2∑i=1n∑j=1m‖m^(K,Ri,ti,Mij)−mij‖2

这是一个非线性优化问题，可以使用Levenberg-Marquardt的方法，利用上面得到的解作为初始值，迭代得到最优解。

消除径向畸变

为了取得好的成像效果，通常要在相机的镜头前添加透镜。在相机成像的过程中，透镜会对光线的传播产生影响，从而影响相机的成像效果，产生畸变：

透镜自身的形状对才光线的传播产生影响，形成的畸变称为径向畸变。在小孔模型中，一条指向在成像平面上的像仍然是直线。但是在实际拍摄的过程中，由于透镜的存在，往往将一条直线投影成了曲线，越靠近图像的边缘，这种现象越明显。透镜往往是中心对称的，使得这种不规则的畸变通常是径向对称的。主要有两大类：桶形畸变和枕形畸变。如下图
由于在相机组装的过程中，透镜不能和成像平面严格平行，会引入切向畸变。

张氏标定法中只关注了影响较大的径向畸变。
设，(μ,ν)(μ,ν)是理想的无畸变的像素坐标；(μ̂ ,ν̂ )(μ^,ν^)是畸变后的像素坐标；(μ0,ν0)(μ0,ν0)是相机的主点；(x,y)(x,y)和(x̂ ,ŷ )(x^,y^)理想的无畸变的归一化的图像坐标和畸变后的归一化图像坐标，使用下面的式子表示径向畸变：

x̂ =x+x[k1(x2+y2)+k2(x2+y2)2]ŷ =y+y[k1(x2+y2)+k2(x2+y2)2]x^=x+x[k1(x2+y2)+k2(x2+y2)2]y^=y+y[k1(x2+y2)+k2(x2+y2)2]

k1,k2k1,k2表示径向畸变的系数。径向畸变的中心和相机的主心是在相同的位置，

μ̂ =μ0+αx̂ +γŷ ν̂ =ν0+βŷ μ^=μ0+αx^+γy^ν^=ν0+βy^

假设γ=0γ=0，则有：

μ̂ =μ+(μ−μ0)[k1(x2+y2)+k2(x2+y2)2]ν̂ =ν+(ν−ν0)[k1(x2+y2)+k2(x2+y2)2]μ^=μ+(μ−μ0)[k1(x2+y2)+k2(x2+y2)2]ν^=ν+(ν−ν0)[k1(x2+y2)+k2(x2+y2)2]

将上面的式子改写为矩阵的形式

[(μ−μ0)(x2+y2)(ν−ν0)(x2+y2)(μ−μ0)(x2+y2)2(ν−ν0)(x2+y2)2][k1k2]=[μ̂ −μν̂ −ν][(μ−μ0)(x2+y2)(μ−μ0)(x2+y2)2(ν−ν0)(x2+y2)(ν−ν0)(x2+y2)2][k1k2]=[μ^−μν^−ν]

上面的等式是从一幅图像上的一个点取得，设有nn幅图像，每幅图像上有mm个点，则将得到的所有等式组合起来，可以得到2mn2mn个等式，将其记着矩阵形式

Dk=dDk=d

则可得

k=[k1 k2]T=(DTD)−1DTdk=[k1 k2]T=(DTD)−1DTd

和上面类似利用最大似然估计取得最优解，使用LM的方法估计使得下面式子是最小值的参数值

∑i=1n∑j=1m‖m̂ (K,k1,k2,Ri,ti,Mij)−mij‖2∑i=1n∑j=1m‖m^(K,k1,k2,Ri,ti,Mij)−mij‖2

得到畸变参数k1,k2k1,k2后，可以先将图像进行去畸变处理，然后用去畸变后的图像坐标估计相机的内参数。

OpenCV中的张正友相机标定

OpenCV中提供了对张正友标定算法的实现，其使用步骤如下：

制作标定使用标定板。标定板很简单，如下图：

可以将其打印下来固定到一张平板上就是标定使用的标定板。使用同一相机从不同的位置，不同的角度，不同的姿态，拍摄标定板的多张照片（一般不少于3张，以10-20张为宜）。

提取标定板的内角点的世界坐标，这里需要注意标定板的大小是标定板在水平和竖直方向上内焦点的个数。内焦点指的是，标定板上不挨着表姐的角点。比如上图制作的标定板的大小是(4,6)(4,6)，其在水平方向有4个内焦点；竖直方向有6个内焦点。标定板所在的世界坐标系为Z=0Z=0，则其角点的世界坐标系如下：

        // 3D场景中的点，在棋盘坐标系中初始化棋盘角点，这些角点的坐标为(x,y,z) = (i,j,0)
        // 棋盘所在的世界坐标系，X轴竖直向下，Y轴水平向右，Z = 0
        for (int i = 0; i < boardSize.height; i++)
            for (int j = 0; j < boardSize.width; j++)
                objectCorner.push_back(Point3f(i, j, 0.0f));

提取标定板的角点的图像坐标，标定板角点的提取可以调用函数findChessboardCorners(image, boardSize, imageCorner);，为了得到更高的精度，可以将提取到的像素坐标精确到亚像素。提取角点图像坐标的代码如下：

        // 2D 图像的像素坐标
        Mat image; //  保存标定板的图像
        int viewPointCount = 0;
        // 从各个视角
        for (int i = 0; i < filelist.size(); i++)
        {
            image = imread(filelist[i], 0);
            // 取得图像的角点
            bool found = findChessboardCorners(image, boardSize, imageCorner);

            // 获取亚像素精度
            cornerSubPix(image, imageCorner, Size(5, 5), Size(-1, -1),
                TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 30, 0.1));

            // 取得的角点数目满足要求，则将它加入数据中
            if (imageCorner.size() == boardSize.area())
            {
                // 将3D场景点及其对应像的像点加入
                addPoints(imageCorner, objectCorner);
                viewPointCount++;
            }
        }

已经取得了角点的世界坐标以及对应的像素坐标，就可以进行标定了，调用calibrateCamera(objectPoints, imagePoints, imageSize, camteraMatrix, distCoeffs, rvecs, tvecs, flag); 这里的cameraMatrix就是要求的相机的内参数矩阵。

上面只是简单的介绍下OpenCV相机标定的使用方法，具体的可以参考官方文档。