在OpenGL中理解摄像机标定

摄像机是3D世界和2D图像之间的一种映射。
利用齐次坐标表示3D空间坐标$\mathbf{X}(X,Y,Z,1)$，2D图像空间坐标$\mathbf{x}(x,y,1)$，存在一个3X4的矩阵$\mathbf{P}$,满足：

$\mathbf{x}=\mathbf{P}\mathbf{X}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1-1)$

如果已知足够数量的x与X一一对应的点，便可以计算出矩阵$\mathbf{P}$。

为了更进一步了解$\mathbf{P}$的具体含义，需要弄清楚针孔摄像机的几何特性。

考虑空间点到一张平面上的中心投影，假设投影中心位于欧式坐标系的原点，而平面$\mathbf{Z}=f$被称为图像平面或聚焦平面。根据三角形相似原理，从世界坐标到图像坐标的中心投影是：

$(X,Y,Z)^T ->(fX/Z,fY/Z)^T\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1-2)$

这是从三维欧氏空间到二维欧氏空间的一个映射。并且假定图像平面的坐标原点在主点上，（摄像机中心到图像平面的垂线称为摄像机的主轴，而主轴与图像平面的交点称为主点，如上图中点p）。一般情况下，存在主点偏置。

$(X,Y,Z)^T ->(fX/Z + p_{x},fY/Z + p_{y})^T\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1-3)$

其中，$(p_{x},p_{y})^T$是在图像坐标系下主点的坐标。

参考图5.2，公式（1-3）用齐次坐标可以表示成：

$Z\left[\begin{array}{ccc} x \\ y \\ 1\end{array}\right] =\left[\begin{array}{ccc} f & 0 & p_{x} & 0 \\ 0 & f & p_{y} & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]\left[\begin{array}{ccc} X \\Y \\Z \\1\end{array}\right] \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1-4)$

需要强调的是，坐标$[X,Y,Z,1]^T$是以摄像机中心为原点建立的坐标下表示的三维空间中的点，并且Z轴为摄像机的主轴方向，此时可记$\mathbf{X}_{cam} =[X,Y,Z,1]^T$，该坐标系称为摄像机坐标系。一般情况下，空间点采用不同的欧氏坐标系表示，称为世界坐标系。摄像机坐标与世界坐标系通过旋转和平移相联系。如图：

如果$\mathbf{\widetilde{X}}$表示世界坐标系中一点的非齐次坐标，$\mathbf{\widetilde{X}}_{cam}$是以摄像机坐标系表示的同一点，则有$\mathbf{\widetilde{X}}_{cam} = \mathbf{R}(\mathbf{\widetilde{X}} - \mathbf{\widetilde{C}})$。其中，$\mathbf{\widetilde{C}}$ 表示摄像机中心在世界坐标系中的坐标，$\mathbf{R}$是一个3X3的旋转矩阵，表示摄像机坐标系的方位，该方程在齐次坐标下可以写成

$X_{cam} = \left[\begin{array}{ccc}R & -R\widetilde{C}\\ \mathbf{0}^T & 1\end{array}\right]\left[\begin{array}{ccc}X\\Y\\Z\\1\end{array}\right] = \left[\begin{array}{ccc}R & -R\widetilde{C}\\ \mathbf{0}^T & 1\end{array}\right]{X} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1-5)$

将公式（1-4）与公式（1-5）合并，

$\mathbf{x}=KR\left[\begin{array}{ccc}\mathbf{I}\ |& -\widetilde{C} \end{array}\right]\mathbf{X}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ （1-6）$.

其中，

$K=\left[\begin{array}{ccc} f & 0 & p_{x} \\ 0 & f & p_{y} \\ 0 & 0 & 1 \end{array}\right]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ （1-7）$

根据（1-1）式，摄像机矩阵可分解为：

$\mathbf{P} = \mathbf{K}\left[\begin{array}{ccc}\mathbf{R} |\mathbf{t}\end{array}\right]\ \ ,\mathbf{t} = -\mathbf{R}\widetilde{C} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ （1-8）$

上面推导的针孔摄像机模型假设图像坐标在两个轴向上有等尺度的欧氏坐标。在CCD摄像机模型中，像素不可能为正方形，如果图像坐标以像素来测量，则需要在每个方向上引入非等量的尺度因子。具体地说，如果在x,y方向上图像坐标单位距离的像素数分别是$m_{x},m_{y}$,那么由世界坐标系到像素坐标系的变换将由（1-7）式左乘一个附加的因子$diag(m_{x},m_{y},1)$而得到。因此，一个CCD摄像机标定矩阵的一般形式为：

$K=\left[\begin{array}{ccc} f_{x} & 0 & x_{0} \\ 0 & f_{y} & y_{0} \\ 0 & 0 & 1 \end{array}\right]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ （1-9）$

其中，$f_{x} = f*m_{x},f_{y}=f*m_{y}$分别把摄像机的焦距换算成x,y方向上的像素量纲，同理，$\mathbf{x_{0}}=(x_{0},y_{0})^T$是用像素量纲表示的主点，$x_{0} = m_{x}*p_{x},y_{0}=m_{y}*p_{y}$。

至此，摄像机的映射已介绍完毕，需要提醒一点的是，在上图5.2中，建立的图像坐标y轴方向是朝上的，其与实际的图像坐标系y轴是反向的。

在OpenCV中，给出了摄像机标定的方法，通过示例，可以准确地计算出矩阵$\mathbf{P}$,$\mathbf{K}$,$\mathbf{R}$以及$\mathbf{t}$。为了更深刻地理解旋转矩阵$\mathbf{R}$以及位移向量$\mathbf{t}$，可以在OpenGL中绘制出参考标定板的位置，然后通过$\mathbf{R}$以及$\mathbf{t}$绘制出对应不同视角下的相机。

关于OpenCV相机的标定看这里。

关于OpenGL的Transformation 看这里。

OpenCV标定程序是sources\samples\cpp目录下的calibration.cpp，使用的图像数据是 left01.jpg ~ left14.jpg ，共13张图片。

如图，识别到的棋盘点的顺序如图编号所示，由于点的坐标是以图片左上角为原点，所以我将坐标原点修改为图像的左下角，此时，图像坐标系与上述图5.2中的保持一致。在calibration.cpp中代码修改如下：

...
//line 487
if( mode == CAPTURING && found &&
        (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) )
   {
        vector<Point2f> points;
        for (int i = 0; i < pointbuf.size(); i++)
        {
            Point2f p = pointbuf[i];    
            points.push_back(Point2f(p.x, imageSize.height - p.y));
        }

        imagePoints.push_back(points);//存储更新坐标系后的点

         prevTimestamp = clock();
         blink = capture.isOpened();
   }
...

同样，在定义棋盘点的空间坐标时，选取的坐标系与图5.2保持一致，从而设定点46为坐标系原点，点1的三维坐标即为（0,5,0），点9的坐标为（8,5,0）(squareSize = 1)。在calibration.cpp中修改代码如下：

...
//line 114
case CHESSBOARD:
      case CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float(j*squareSize),
                float((boardSize.height - 1 -  i)*squareSize), 0));
        break;
...

命令运行标定程序：

E:\OpenCV\Sample\camCalib\bin\bin>cameraCalib.exe -w 9 -h 6 -o camera.yml -oe imagelist.xml

生成的camera.yml包括摄像机的内参数以及在不同视角下的旋转和位移。旋转通过单一的旋转角 $\theta$ 和所围绕的单位向量方向$\hat{\mathbf{v}} = (x,y,z)$来定义，在OpenCV 中的Rodrigues方法可以看到详细说明：

此时，因为角-轴表示密切关联于四元数表示。依据轴和角，四元数可以给出为正规化四元数 Q :

$Q=(xi+yj+zk)\sin(\theta/2)+\cos(\theta/2)$

然后，根据四元数与欧拉角的关系，计算出绕Z轴、Y轴、X轴的旋转角度$\psi,\theta,\varphi$。

由公式（1-8），不难求出摄像机的中心坐标，$\mathbf{C}=-\mathbf{R}^{-1}\mathbf{t}$。
代码如下：

...
int i = cameraId;
Mat camPos = tvecs[i]; //获取第i个相机的位移向量
Mat camRot = rvecs[i]; //获取第i个相机的旋转向量

Mat rotation;
cv::Rodrigues(camRot, rotation);  

Mat cameraPos = -rotation.inv()*camPos; // C = -R^-1*t
std::cout << "Cam Position: " << cameraPos << std::endl;

float w, x, y, z;
w = cos(norm(camRot) / 2);
x = sin(norm(camRot) / 2)*camRot.ptr<double>(0)[0] / norm(camRot);
y = sin(norm(camRot) / 2)*camRot.ptr<double>(1)[0] / norm(camRot);
z = sin(norm(camRot) / 2)*camRot.ptr<double>(2)[0] / norm(camRot);

std::cout << w << " " << x << " " << y << " " << z << std::endl;
//四元素转欧拉角
cameraAngle[0] = atan2(2 * (w*x + z*y), 1 - 2 * (y*y + x*x));
cameraAngle[1] = asin(2 * (w*y - x*z));
cameraAngle[2] = atan2(2 * (w*z + y*x), 1 - 2 * (z*z + y*y));

//欧拉角反推旋转矩阵，进行验证
Matrix3 Rx, Ry, Rz;
Rx.set(1, 0, 0, 0, cos(cameraAngle[0]), -sin(cameraAngle[0]), 0, sin(cameraAngle[0]), cos(cameraAngle[0]));
Ry.set(cos(cameraAngle[1]), 0, sin(cameraAngle[1]), 0, 1, 0, -sin(cameraAngle[1]), 0, cos(cameraAngle[1]));
Rz.set(cos(cameraAngle[2]), -sin(cameraAngle[2]), 0, sin(cameraAngle[2]), cos(cameraAngle[2]), 0, 0, 0, 1);

Matrix3 R = Rz*Ry*Rx;
std::cout << rotation << std::endl;
std::cout << R << std::endl << std::endl;

...

利用计算得到的摄像机位置与姿态，处理如下：

...
glPushMatrix();
glTranslatef(cameraPos.at<double>(0, 0), cameraPos.at<double>(1, 0), -cameraPos.at<double>(2, 0));  //OpenGL的Z轴与相机自身Z轴是反向的。
glRotatef(cameraAngle[0] * 180 / 3.1415926, 1, 0, 0);
glRotatef(cameraAngle[1] * 180 / 3.1415926, 0, 1, 0);
glRotatef(cameraAngle[2] * 180 / 3.1415926, 0, 0, 1);

drawCamera();
drawFrustum(FOV_Y, 4.0/3.0, 1, 20);
glPopMatrix();
...

为了将摄像机参数文件camera.yml 的相关数据读入程序，提供了如下方法：

void readCamerasParams(const string filename, std::vector<Mat>& rvecs, std::vector<Mat>& tvecs)
{
    FileStorage fs(filename, FileStorage::READ);

    int nframes;
    fs["nframes"] >> nframes;

    Mat bigmat;

    fs["extrinsic_parameters"] >> bigmat;

    rvecs.resize(bigmat.rows);
    tvecs.resize(bigmat.rows);

    for (int i = 0; i < bigmat.rows; i++ )
    {
        Mat r = bigmat(Range(i, i + 1), Range(0, 3));
        Mat t = bigmat(Range(i, i + 1), Range(3, 6));

        rvecs[i] = r.t();
        tvecs[i] = t.t();

    }

    for (int i = 0; i < rvecs.size();i++)
    {
        std::cout << rvecs[i] << " ," << tvecs[i] << std::endl;
    }
}

结果展示：
0号相机_left01.jpg：

5号相机_left06.jpg：

全部相机：