相机的标定是 SLAM 最开始的部分，由于设备原因，这个星期只做了手机相机的标定。这篇文章主要就是介绍一下相机标定的原理以及用OpenCV中现有的函数或是Matlab做相机标定的过程。

0. 资料

先把相机标定过程中看过的资料摆一下：
摄像机内参标定《A Flexible New Technique for Camera Calibration》；
摄像机-激光雷达静态外参联合标定《Extrinsic calibration of a camera and laser range finder (improves camera calibration)》；
注意结合运动信息，物体的运动与激光雷达的旋转扫描同时发生；
运动补偿激光雷达与相机之间的标定；

在本篇报告中只用到了第一篇资料中所述的内容。

1. 原理

以下步骤出自《A Flexible New Technique for Camera Calibration》

设一个 2D 平面上的点 $\text{m}=[u,v{]}^T$，与之相对应的 3D 空间中的点为 $M=[X,Y,Z{]}^T$ 。则根据针孔相机的模型，我们有：
$$s\text{m}=\text{A}\left[\begin{array}{cc}\text{R}& \text{t}\end{array}\right]M$$

这里式子中的 $\text{m}$ 与 $M$ 均用了齐次坐标的形式，即 $\text{m}=[u,v,1{]}^T$ ，$M=[X,Y,Z,1{]}^T$ 。$\left[\begin{array}{cc}\text{R}& \text{t}\end{array}\right]$ 为相机的位姿（Extrinsic），$\text{A}$ 为相机的内参矩阵（Intrinsic matrix）：
$$\text{A}=\left[\begin{array}{ccc}\alpha & \gamma & u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$$

我们标定时采用黑白方格的标定版，则 $M$ 的的坐标我们是已知的，令 $Z=0$，则有：
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\text{A}\left[\begin{array}{cccc}{\text{r}}_1& {\text{r}}_2& {\text{r}}_3& \text{t}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\text{A}\left[\begin{array}{ccc}{\text{r}}_1& {\text{r}}_2& \text{t}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$$

令 $M=[X,Y,1{]}^T$ ，$s\text{H}=\text{A}\left[\begin{array}{ccc}{\text{r}}_1& {\text{r}}_2& \text{t}\end{array}\right]$，则有 $\text{m}=\text{H}M$。
把 $\text{H}$ 记成 $\left[\begin{array}{ccc}{\text{h}}_1& {\text{h}}_2& {\text{h}}_3\end{array}\right]$ ，则有：
$${\text{h}}_1^T{\text{A}}^{-T}{\text{A}}^{-1}{\text{h}}_2=0$$$${\text{h}}_1^T{\text{A}}^{-T}{\text{A}}^{-1}{\text{h}}_1={\text{h}}_2^T{\text{A}}^{-T}{\text{A}}^{-1}{\text{h}}_2$$

设:
$$\text{B}={\text{A}}^{-T}{\text{A}}^{-1}=\left[\begin{array}{ccc}{B}_{11}& B_{12}& B_{13}\\ B_{12}& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]$$

故 $\text{B}$ 可以由一个 6D 向量定义：
$$\text{b}=[B_{11},B_2,B_{22},B_{13},B_{23},B_{33}{]}^T$$

令 ${\text{h}}_{i,i=1,2,3}=[h_{i1},h_{i2},h_{i3}{]}^T$，则有：
$${\text{h}}_i^T\text{B}{\text{h}}_j={\text{v}}_{ij}\text{b}$$

其中:
$${\text{v}}_{ij}=[h_{i1}{h}_{j1},h_{i1}{h}_{j2}+h_{i2}{h}_{j1},h_{i2}{h}_{j2},h_{i3}{h}_{j1}+h_{i1}{h}_{j3},h_{i3}{h}_{j2}+h_{i2}{h}_{j3},h_{i3}{h}_{j3}{]}^T$$

上式也可以写成：
$$\left[\begin{array}{c}{\text{v}}_{12}^T\\ ({\text{v}}_{11}-{\text{v}}_{22}{)}^T\end{array}\right]\text{b}=\text{0}or\text{V}\text{b}=0$$

式子中， $\text{V}$ 是我们可以测得的多组数据，根据上式便可以简单估计出 $\text{b}$ 的取值，从而求出 $\text{A}$ 与 $\text{H}$ 。在求出了这些量之后，我们还可以求出相机的位姿，即外参矩阵。

但正如《A Flexible New Technique for Camera Calibration》中所说，上述步骤求得内参矩阵使用了使得代数距离最小，这并没有什么物理意义，只是给了接下来的测量一个初始值。于是在实践中我们采用最大似然估计，在这里是最小化重投影误差。

误差项可以写为：
$${\text{e}}_{ij}={\text{m}}_{ij}-\text{f}(\text{A},{\text{R}}_i,{\text{t}}_i,M_j)$$其中 $\text{f}$ 为点 $M_j$ 到相机图像平面的重投影函数。

考虑径向畸变：
$$\{\begin{array}{c}{x}_{corrected}=x(1+k_1{r}^2+k_2{r}^2)\\ \\ y_{corrected}=y(1+k_1{r}^2+k_2{r}^2)\end{array}$$

则误差项变为：
$${\text{e}}_{ij}={\text{m}}_{ij}-\text{g}(\text{A},k_1,k_2,{\text{R}}_i,{\text{t}}_i,M_j)$$

其中 $\text{g}$ 为点 $M_j$ 到相机图像平面的重投影函数。

于是我们便得到了一个最小二乘问题：
$$\sum _{i=1}^n\sum _{j=1}^m||{\text{e}}_{ij}|{|}^2$$

这个问题应该可以用G2O或者Ceres来求解（我还没有实现用这些库的求解）。

---

2. 实现过程

我用的是黑白格的标定板，直接放在Ipad上，然后pad放地上不动，使图片中的所有点都在同一个平面上。标定版的大小是 $8\times 11$ ，一奇一偶的大小是为了使OpenCV找角点的函数或者是Matlab能够确定起始点的位置。照片如下：

2.1. OpenCV实现过程

因为 OpenCV 直接找角点和相机标定的函数，所以就直接用了。代码如下：

#include "common_include.h"
#include <boost/format.hpp>
#include "config.h"
#include <string>

int main( int argc, char** argv ) {
    

	//set parameters
	boost::format fmt( "%d.jpg" );
	Config::setParameterFile("../config/default.yaml");
	int h = Config::get<int> ("Size.height");
	int w = Config::get<int> ("Size.width");
	int img_num = Config::get<int> ("ImageNumber");
	int image_h = Config::get<int> ("Image.height");
	int image_w = Config::get<int> ("Image.width");
	string image_path = Config::get<string> ("ImagePath");
	float scale = Config::get<float> ("Scale");
	
	Size boardSize(w, h);
	Size imageSize(image_w, image_h);
	cout << "---------------------------------------------------------\n";
	cout << "The board size is " << boardSize.width << " * " << boardSize.height << endl;
	cout << "---------------------------------------------------------\n";
	cout << "The image size is " << imageSize.width << " * " << imageSize.height << endl;
	cout << "---------------------------------------------------------\n";
	
	//initialization
	Mat img;
	vector<Point3f> objects;
	vector<vector<Point2f>> imageCorners;
	vector<vector<Point3f>> objectCorners;
	vector<Point2f> corners;
	int temp = 0;
	
	//The points in world coodinate
	for (int i=0; i<h; i++)
		for (int j=0; j<w; j++)
			objects.push_back( Point3f(j, i, 0.0) );

	for (int i=0; i<img_num; i++) {
    
		cout << "Read the image \"" << ((fmt%(i+1)).str()) << "\" ...\n";
		img = imread( image_path + (fmt%(i+1)).str(), 0 );
		if ( img.empty() ) {
    
			cout << "The image \"" << ((fmt%(i+1)).str()) << "\" does not exits!\n\n";
			continue;
		}
		resize( img, img, Size(), scale, scale, INTER_AREA );
		
		cout << "Find the corner of the image ...\n";
		bool found = findChessboardCorners( img, boardSize, corners );
		
		if (found) {
    
			cornerSubPix( img, corners, Size(5, 5), Size(-1, -1),
				cv::TermCriteria(
					cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS,
                	30, 0.01
            	)
        	);
        	
        	for (int i=0; i<corners.size(); i++) {
    
        		corners[i] *= 1/scale;
        	}        	
        	cout << "Add the corner of the image ...\n";
			if ( h*w == corners.size() ) {
    
				imageCorners.push_back( corners );
				objectCorners.push_back( objects );
				temp++;
			}
			else cout << "The image is rejected!\n\n";
        }
		else cout << "The corners does not found!\n\n";
	}
	/*
	cout << "---------------------------------------------------------\n";
	cout << objectCorners[0] << endl << imageCorners[0];
	*/
	cout << "---------------------------------------------------------\n";
	cout << "The total number of images which is successfully readed is: " << temp << endl;
	cout << "---------------------------------------------------------\n";
	
	Mat A = (Mat_<float>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
	Mat D = (Mat_<float>(4, 1) << 0, 0, 0, 0);
	vector<Mat> R;
	vector<Mat> t;
	
	calibrateCamera(objectCorners, imageCorners, imageSize, A, D, R, t);
	
	double error=0;
	double total=0;
	for (int i=0; i<R.size(); i++) {
    
		vector<Point2f> repoints;
		projectPoints(objectCorners[i], R[i], t[i], A, D, repoints);
		total += repoints.size();
		for (int j; j<repoints.size(); j++) error += norm(imageCorners[i][j] - repoints[j]);
	}
	
	cout << "The intrinsic matrix is: \n" << A <<endl;
	cout << "---------------------------------------------------------\n";
	cout << "The distortion vector is: \n" << D << endl;
	cout << "---------------------------------------------------------\n";
	cout << "The reproject error is: " << error/total << endl;
	cout << "The total number of points is: " << total << endl;
	return 0;
}

实现过程中的问题与解决： 这里我写了一个 Config 的类来读取一些初始值，具体代码在《SLAM 14讲》的第九章，我也是直接拿来用的。由于开始的时候很多图片找不到角点，所以我在读取图片读取角点时都会在屏幕上输出相关的文字以判断是否成功读取了。后来发现读取不到角点是因为手机拍的图片太大了，于是我在读取图片后先把图片缩小，测出角点坐标后再把坐标按相同的比例放大。另外要注意的是，3D 点的顺序和角点的顺序必须一一对应，于是在生成 3D 点的坐标 vector 时顺序是从左往右，由上至下。

以下时运行的结果：

2.2 Matlab 实现过程

Matlab 有直接的APP可以直接作相机的标定，直接把图片放进去就可以了。结果如下：

2.3. 总结和问题

可以看出不论是哪种方法，手机相机的内参矩阵几乎是一样的。这间接的可以说明我们得到的结果还是比较准确的。
但用 Matlab 时得到的平均重投影误差比较大，我现在还没找到缩小的办法。另外，两个方法测出来的径向畸变的系数也有较大不同，这可能是因为这个系数本来较小，所以显示出来的相对误差就会比较大。

3. OpenCV的补充

摘抄一段官网上的说明

double cv::calibrateCamera 	( 	InputArrayOfArrays  	objectPoints,
								InputArrayOfArrays  	imagePoints,
								Size  					imageSize,
								InputOutputArray  		cameraMatrix,
								InputOutputArray  		distCoeffs,
								OutputArrayOfArrays  	rvecs,
								OutputArrayOfArrays  	tvecs,
								OutputArray  			stdDeviationsIntrinsics,
								OutputArray  			stdDeviationsExtrinsics,
								OutputArray  			perViewErrors,
								int  					flags = 0,
								TermCriteria  			criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, DBL_EPSILON) 
	) 	

objectPoints： 输入参数，类型类似 vector< vecotor < Point3f > > 。
imagePoints： 输入参数，类型类似 vector< vecotor < Point2f > > 。
imageSize： 输入参数，类型是 cv::Size 。
cameraMatrix： 输入输出参数，类型是 cv::Mat 。
distCoeffs： 输入输出参数，类型是 cv::Mat 。
rvecs： 输出参数，类型是 vector< cv::Mat > 。
tvecs： 输出参数，类型是 vector< cv::Mat > 。
stdDeviationsIntrinsics： 相机内参矩阵和畸变系数的偏差值。
stdDeviationsExtrinsics： 相机外参的偏差值。
perViewErrors： 每张图的重投影误差。
flags：

• CALIB_USE_INTRINSIC_GUESS： 标定时使用初始值。
• CALIB_FIX_PRINCIPAL_POINT： 中心点不变。
• CALIB_FIX_ASPECT_RATIO： $\frac{f_x}{f_y}$ 的值不变。
• CALIB_ZERO_TANGENT_DIST： 切向畸变值设为0。
• CALIB_FIX_K1,…,CALIB_FIX_K6： 相应的径向畸变系数不变，若不是用初始值，则设为0。
• CALIB_RATIONAL_MODEL： 使用 $k_4,k_5,k_6$ ，若没有这个 distCoeffs 只返回五个参数。
• CALIB_THIN_PRISM_MODEL： 使用 $s_1,s_2,s_3$ 。
• CALIB_FIX_S1_S2_S3_S4： 固定 $s_1,s_2,s_3$ 。
• CALIB_TILTED_MODEL： 使用 ${\tau }_x,{\tau }_y$ 。
• CALIB_FIX_TAUX_TAUY: 固定 ${\tau }_x,{\tau }_y$ 。