整理一下关于单目摄像头矫正的内容，方便理解单目相机矫正得到的参数。

凸透镜成像到小孔成像

我们平时卖到的摄像头由两个部分组成，感光元件和镜头：


感光元件作用就是把光学影像转化为数字信号，这个很好理解，关于矫正的工作在其实都在镜头上。
摄像机镜头相当于是一个凸透镜。回顾一下凸透镜成像的知识：

在成像中我们都只讨论上面的第一种情况，当物距离u、像距v、焦距f满足u>2f、f<v<2f时，获得缩小、倒立的成像。
借助教材PPT上的一页：

得到比例关系：
\frac{1}{f} = \frac{1}{u} + \frac{1}{v}
不同于凸透镜成像是遵循光的折射规律，小孔成像是遵循光沿直线传播的规律。

很明显我们平时使用的摄像头是基于凸透镜的原理来的，但是为什么大家在计算的时候都使用小孔模型呢？
这里有一个假设：当物距u足够大的时候（也就是成像物体足够远），\frac{1}{f} = \frac{1}{u} + \frac{1}{v}里的1/u趋近于0，焦距f等于相距v。
直观上就是景深趋近于无穷，凸透镜的孔径趋近于零，它可以认为是一个小孔成像模型，对应上图公式h_{2} = (d_{2}/d_{1})\cdot h_{1}里的 d1 = u，d2 = f。

从世界坐标系到像素坐标系

根据凸透镜与小孔成像的了解，我们可以将三维空间上一点投影到相机成像平面的方式画出来：

如上图，假设三维空间上一点p(x,y,z)经过光心O投影到成像平面上得到点{p}’ ({X}’,{Y}’)。根据几个相似三角形关系，很容易得到：\frac{{X}’ }{X} =\frac{{Y}’ }{Y} =\frac{f}{Z}
以上是点在世界坐标系下到成像平面之间的坐标变换。
而我们平时所说的像素坐标系与成像平面并不是一个坐标系，这两个坐标系存在一个缩放和平移。
这里解释一下缩放和平移（注意都是二维平面和坐标系）：
关于缩放，我们可以理解为，由于镜头制作安装过程中不能保证与相机感光芯片完全平行，而我们假设的的成像平面是与镜头平行的。也就是说：我们假设的成像平面与实际的成像平面不平行。这种不平行也就带来x轴和y轴有各自不同的缩放\alpha 和\beta ，这个缩放的不同也是我们相机矫正为什么会有两个焦距的原因。
在这里，上图的视角可能有点问题，是从前面往后面看的，在研究过程中视角应该是从从相机背面往前面看，如下：

关于平移，就好理解一些，成像平面坐标系的的坐标原点在平面中心，而像素坐标系的坐标原点在平面的左上角，我们假设这个平移为c_{x} 和c_{y} 。
所以对于我们假设的成像平面坐标系上一点{p}’ ({X}’,{Y}’)与其对应的像素坐标系下一点{p}’’(u,v)的关系为：\left\{\begin{matrix} u=\alpha {X}’ + c_{x} \\v=\beta {Y}’ + c_{y}\end{matrix}\right.
带入前面得到的\frac{{X}’ }{X} =\frac{{Y}’ }{Y} =\frac{f}{Z}
得到:
\left\{\begin{matrix} u=\alpha\cdot f\cdot \frac{X}{Z} + c_{x} \\v=\beta \cdot f\cdot \frac{Y}{Z} + c_{y}\end{matrix}\right.
将\alpha\cdot f与\beta\cdot f替换为f_{x} 与f_{yx} ：
\left\{\begin{matrix} u=f_{x} \cdot \frac{X}{Z} + c_{x} \\v=f_{y} \cdot \frac{Y}{Z} + c_{y}\end{matrix}\right.
转换为矩阵形式有：
Z\cdot \begin{bmatrix} u\\ v\\1\end{bmatrix}=\begin{bmatrix}f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0& 0&1\end {bmatrix}\cdot\begin{bmatrix} X\\ Y\\Z\end{bmatrix}
其中p(X,Y,Z)为点在世界坐标系下的点，{p}’’(u,v)为该点投影到像素坐标系下的点，矩阵\begin{bmatrix}f_{x} & 0 & c_{x}\\ 0 & f_{y} & c_{y}\\ 0& 0&1\end {bmatrix}被称为内参矩阵

相机畸变

径向畸变

径向畸变是由透镜形状导致的，是透镜自身形状对光线传播的影响。
在针孔模型中一条直线投影到像素平面上还是一条直线，可以在实际中，透镜往往会使得真实环境中的一条直线变成一条曲线，曲线往里弯就是桶形畸变、往外弯就是枕形畸变。

径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点发生了变化。可以把畸变看作 r=0 附近的泰勒奇数展开的前几项来便是。一般为前两项 k1 , k2，对于鱼眼透镜 ，会用前三项 k3 。成像装置上某点的径向位置可以根据以下等式进行调整，这时便有了3个或2个的未知变量。
\left\{\begin{matrix}x_{distorted} = x(1 + k_{1}r^{2} + k_{2}r^{4} + k_{3}r^{6}) \\y_{distorted} = y(1 + k_{1}r^{2} + k_{2}r^{4} + k_{3}r^{6}) \end{matrix}\right.

切相畸变

切向畸变是由透镜与成像平面位置不平行所导致的，这也会使得光线穿过透镜投影到成像平面时的位置发生变化。

切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。切向畸变可以用两个参数p1 和 p2 来表示：
\left\{\begin{matrix}x_{distorted} = x + 2p_{1}xy + p_{2}(r^{2} + 2x^{2}) \\y_{distorted} = y + p_{1}(r^{2} + 2y^{2}) + 2p_{2}xy\end{matrix}\right.

综合以上两个式子，有：
\left\{\begin{matrix}x_{distorted} = x(1 + k_{1}r^{2} + k_{2}r^{4} + k_{3}r^{6}) + 2p_{1}xy + p_{2}(r^{2} + 2x^{2}) \\y_{distorted} = y(1 + k_{1}r^{2} + k_{2}r^{4} + k_{3}r^{6}) + p_{1}(r^{2} + 2y^{2}) + 2p_{2}xy\end{matrix}\right.

代入针孔相机模型再有：
\left\{\begin{matrix} u=f_{x} \cdot \frac{X_{distorted}}{Z} + c_{x} \\v=f_{y} \cdot \frac{Y_{distorted}}{Z} + c_{y}\end{matrix}\right.
一般来说，我们是先对图像进行去畸变，得到去畸变后的图像，然后再讨论像素坐标系上的位置到空间位置上的映射。

标定方法

关于单目相机标定的原理，大家具体可以去看张正友大大的《A Flexible New Technique for Camera Calibration》。

单应矩阵构建与求解

将外参矩阵记为W，代表点从世界坐标系转到相机坐标系的旋转和平移矩阵[R,t],P_{C}为点在相机坐标系下的位置、 P_{W}为点在世界坐标系下的位置。这里表示的方式与我们平时的不一样，P_{C}没有做齐次变换
P_{C} =\begin{bmatrix} R & t\end{bmatrix}\cdot\begin{bmatrix} P_{W}\\1\end{bmatrix}=W\cdot\begin{bmatrix} P_{W}\\1\end{bmatrix}
在张正友方法里，设点在世界坐标系下的坐标Z=0，也就是说世界坐标系的X轴、Y轴与棋盘坐标系的X轴、Y轴在一个平面上，相当于一个降维操作。
在这样处理后，再将坐标展开，将旋转矩阵R用三个列向量表示为R=[r1 r2 r3]可以得到：
\begin{bmatrix} X_{C}\\ Y_{C}\\ Z_{C}\\\end{bmatrix} =\begin{bmatrix} r1&r2&r3&t\end{bmatrix}\cdot\begin{bmatrix} X_{W}\\ Y_{W}\\ Z_{W}=0\\ 1\\\end{bmatrix}
即：
\begin{bmatrix} X_{C}\\ Y_{C}\\ Z_{C}\\\end{bmatrix} =\begin{bmatrix} r1&r2&t\end{bmatrix}\cdot\begin{bmatrix} X_{W}\\ Y_{W}\\\ 1\\\end{bmatrix}
不考虑畸变的情况下，内参矩阵记为A，根据之前的针孔相机模型有：
Z_{C}\cdot \begin{bmatrix} u\\ v\\1\end{bmatrix}= A\cdot\begin{bmatrix} X_{C}\\ Y_{C}\\ Z_{C}\\\end{bmatrix}
在张正友论文里，将以上等式的Z_{C}表示为尺度因子s（这是一个常数），整理以上两个式子，也就有：
s\cdot \begin{bmatrix} u\\ v\\1\end{bmatrix}= A\cdot\begin{bmatrix} r1&r2&t\end{bmatrix}\cdot\begin{bmatrix} X_{W}\\ Y_{W}\\\ 1\\\end{bmatrix}
将s移项，可以得到：
\begin{bmatrix} u\\ v\\1\end{bmatrix}=H\cdot\begin{bmatrix} X_{W}\\ Y_{W}\\\ 1\\\end{bmatrix}
其中
H=\frac{1}{s} \cdot A\cdot\begin{bmatrix} r1&r2&t\end{bmatrix}
可以看出，H即为一个三乘三的单应矩阵，总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供8个约束方程就可以求解。
（u,v）是像素坐标系下的标定板角点的坐标，通过角点检测算法得到；（x,y）是世界坐标系下的标定板角点的坐标，通过棋盘小格子边长和连接关系得到，例如我将世界坐标系原点放在棋盘标定板的左上第一个角点其坐标为（0，0）那么其它角点坐标可以直接算出来。
每一个标定板的角点可以通过和构建出两个约束方程，由此一张完整的棋盘标定板图像上检测到4个角时就可以求出此图像所对应的矩阵H，一般情况下一张棋盘标定板图片上所检测出的角点数量不止4个，此时可以利用最小二乘法通过非线性优化手段得出矩阵H的最优值。

提取内参矩阵A

将H用三个列向量表示为H=\begin{bmatrix} h_{1}&h_{2} &h_{3}\end{bmatrix}也就有：
\begin{bmatrix} h_{1}&h_{2} &h_{3}\end{bmatrix}=\lambda \cdot A\cdot \begin{bmatrix} r1&r2 &t\end{bmatrix}
r1和r2作为旋转矩阵的两列，存在单位正交的关系，满足：
\left\{\begin{matrix} r1^{T} \cdot r2=0\\ r1^{T} \cdot r1=r2^{T} \cdot r2=1\end{matrix}\right.
也就可以得到论文里提到的，这里将这个公式标记为公式（1）：

我们记B= A^{T} A^{-1}，因为矩阵A是一个上三角矩阵，则B为一个三乘三的对称矩阵，表示为：

去除重复的元素，令：

在前面我们对每个棋盘图像，各自求得一个H=\begin{bmatrix} h_{1}&h_{2} &h_{3}\end{bmatrix}，H为一个六乘六的矩阵，所以\begin{bmatrix} h_{1}&h_{2} &h_{3}\end{bmatrix}其中的每一个元素都是一个三乘一的列向量，记作h_{i}=\begin{bmatrix} h_{i1}&h_{i2} &h_{i3}\end{bmatrix} ，例如 h_{1}=\begin{bmatrix} h_{11}&h_{12} &h_{13}\end{bmatrix}，在这个地方需要去理解一下。
注意 h_{i}与h_{i1} 之间的区别，h的下标一位数字的是一个三乘一的列向量，h的下标两位数字的是一个常数！！！
注意 h_{i}与h_{i1} 之间的区别，h的下标一位数字的是一个三乘一的列向量，h的下标两位数字的是一个常数！！！
注意 h_{i}与h_{i1} 之间的区别，h的下标一位数字的是一个三乘一的列向量，h的下标两位数字的是一个常数！！！
根据上面B与b两个表达式，可以有：

带入公式（1），总结一下就是：

由于矩阵H已知，矩阵v又全部由矩阵H的元素构成，因此矩阵v已知。此时，我们只要求解出向量b，即可得到矩阵B。每张标定板图片可以提供一个以上公式的约束关系，该约束关系含有两个约束方程。但是，向量b有6个未知元素，因此，我们只要取3张标定板照片，得到3个的约束关系，即6个方程，即可求解向量b，也就得到了矩阵B。
再通过B= A^{T} A^{-1}的关系再求解相机的内参矩阵A。

这里的相机内参A的表示方式也值得注意一下，我们平时用的内参表示为：

而在张正友论文里被表示为：

咋一看似乎有点儿不对，论文里有解释：

获得外参矩阵

当标定板图片的个数大于3时（事实上一般需要15到20张标定板图片），可采用最小二乘拟合最佳的向量，为后面的非线性优化做准备，我们需要得到相机的外参[R,t]，这里也用到了旋转矩阵的部分性质：

非线性优化

关于对结果的非线性优化，我们构建最小二乘问题如下：

其中，n代表每一张棋盘标定板图像的数量，m代表每一张图像标定板上角点的数量，注意公式里的几个m不是同一个意思。
m(ij)代表的是点在像素坐标系下的位置，m’(A,R(i),t(i),M(j))是M(j)经过相机外参与内参投影后的点在像素坐标系下的位置。针对整个标定过程，A应该是唯一的；针对每一张棋盘标定板图像，其外参[R,t]是唯一的；对每个点，其m(ij)与M(j)是唯一的。其实这里M(j)表示为M(ij)更好一点的样子。通过LM算法(Levenberg-Marquardt Algorithm)求解这个最小二乘问题。

关于畸变参数

畸变参数也在上面的最小二乘问题中被优化，在进行内参矩阵和外参矩阵的求解的时候，我们假设不存在畸变；在进行畸变系数的求解的时候，认为计算得到的内参矩阵和外参矩阵是不存在误差的。原文是这么说的：

整个方法流程总结

  1. 打印相机标定板
  2. 对标定板从各个不同的角度和位置进行拍照
  3. 检测图像中的特征点
  4. 求解内参和外参
  5. 通过最小二乘问题优化所有参数，包括镜头畸变参数

开始标定

单目相机的标定例程在Opencv的官网有opencv说明，其源码在opencv源代码的samples/cpp/tutorial_code/calib3d/camera_calibration/目录下
ROS也有对应的工具包，camera_calibration，也是在Opencv的基础上封装的，相当好用，非常推荐使用。
我们也可以自己写对应的代码，这里参考知乎大佬的一篇文章

获取标定板子参考opencv文档，注意它的角点排列，我这里是9*6，最外圈不算；以及其每个方格的实际大小，单位为mm

Opencv标定程序

主要函数为calibrateCamera，它有几个参数，在API文档里也有说明：




第一个参数objectPoints是输入参数，代表角点在世界坐标系下的三维坐标，双层vector ，对应一张图片有若干角点，一次矫正有若干图片，这个三维坐标在前面提到，Z=0，X和Y坐标值可以直接根据棋盘的格式计算出来。
第二个参数imagePoints为输入参数，是角点在像素坐标系的二维像素坐标，双层vector ，对应一张图片有若干角点，一次矫正有若干图片，这个坐标是通过 opencv 的 findChessboardCorners 函数寻找棋盘角点得到。
imagePoints[i].size() 必须等于objectPoints[i].size()。换句话讲，就是要保证每次检测都要检测到我的标定板子上的每一个角点，多一个少一个都不行
第三个参数imageSize是输入参数，为每张输入图像的大小。
第四个参数cameraMatrix是输出参数，是相机内参矩阵，3_3大小。
第五个参数distCoeffs是输出参数，是相机畸变参数，n_1大小的矩阵，n=4, 5, 8, 12 or 14 。
第六、第七个参数rvecs、tvecs是输出参数，是每张棋盘标定板的外参，即相机坐标系到棋盘坐标系的欧式变换。
第八个参数flags代表不同的矫正方式，这个一般默认就行。
最后一个参数也是默认就行，没研究过。
主要代码：

void Calibrate::run(void){

  // 标定板图像存放位置
  #define PATH "../pic/*"

  // 定义棋盘上角点的分布，注意我这里是 9*6 的棋盘
  int corners_xn = 9, corners_yn = 6;

  // 定义棋盘上小方块的边长，这个的数值不影响标定的内参矩阵与畸变矩阵的结果，具体看算法原理，可以当做一个比例因子被约去了
  double corners_step = 1;

  // 定义用来保存导入的图片
  Mat image_in; 

  // 定义用来保存文件路径的容器
  std::vector<cv::String> filelist;

  // 定义用来保存旋转和平移矩阵的容器
  vector<Mat> rvecs, tvecs;

  // 定义相机矩阵，畸变矩阵
  Mat cameraMatrix;
  Mat distCoeffs;
  int flags = 0;

  // 定义保存图像二维角点的容器，即点在像素坐标系下的二维像素坐标
  vector<Point2f> corners;

  // 定义保存图像二维角点的容器，即点在像素坐标系下的二维像素坐标。第一层vector代表每张图像的角点，第二层vector代表不同的图像
  vector<vector<Point2f> > corners2;

  // 定义保存图像三维角点的容器，即点在相机坐标系下的三维位置坐标
  vector<Point3f> worldPoints;

  // 定义保存图像三维角点的容器，即点在相机坐标系下的三维位置坐标。第一层vector代表每张图像的角点，第二层vector代表不同的图像
  vector<vector<Point3f> > worldPoints2;


//*********************** 1.根据棋盘格式，计算棋盘标定板上三维点的坐标，注意排列：逐行从左到右 *************************
  for(int j = 0; j < corners_yn; j++){
      for(int k = 0; k < corners_xn; k++){
          worldPoints.push_back(Point3f(k*corners_step ,j*corners_step ,0.0f));           
      }
  }


//*************** 1.读取一个文件夹中的所有图片（所有标定图片）**********************
  glob(PATH, filelist); 


//*************************** 2.遍历图像，为每一张图像找角点 **********************
  for(int i = 0; i < filelist.size(); i++){

    std::cout << filelist[i] << std::endl;

    // 一张张读入图片；
    image_in = imread(filelist[i]);

    // 找图片的角点，参数分别为：
    // 输入图片，图片内角点数（不算棋盘格最外层的角点），输出角点，注意我这里是 9*6 的棋盘，角点排列：逐行从左到右
    bool found = findChessboardCorners(image_in, Size(9,6),corners,CALIB_CB_ADAPTIVE_THRESH|CALIB_CB_NORMALIZE_IMAGE);

    // 将每一张图片上找到的角点在像素坐标系上的二维像素坐标作为一个vector保存
    corners2.push_back(corners);

    //画出角点
    drawChessboardCorners(image_in,Size(9,6),corners, found);

    //显示图像
    imshow("test",image_in);

    // 图像刷新等待时间，单位ms
    waitKey(100);

    // 将每一张图片上找到的角点在相机坐标系下的三维坐标作为一个vector保存
    worldPoints2.push_back(worldPoints);
  }


//*********************** 4.标定 *************************
calibrateCamera(worldPoints2,corners2,image_in.size(),cameraMatrix,distCoeffs,
                    rvecs,tvecs);


//************************************* 5.查看参数 *****************************************
  cout << endl;
  cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << endl;
  cout << cameraMatrix.at<double>(0,0) << " " << cameraMatrix.at<double>(0,1) << " " << cameraMatrix.at<double>(0,2) << endl;
  cout << cameraMatrix.at<double>(1,0) << " " << cameraMatrix.at<double>(1,1) << " " << cameraMatrix.at<double>(1,2) << endl;
  cout << cameraMatrix.at<double>(2,0) << " " << cameraMatrix.at<double>(2,1) << " " << cameraMatrix.at<double>(2,2) << endl;

  cout << endl;
  cout << "DistCoeffs: " << endl;
  cout << distCoeffs.rows << "x" <<distCoeffs.cols << endl;
  for(int i = 0;i < distCoeffs.cols;i++)
  {
      cout << distCoeffs.at<double>(0,i) << " " ;
  }
  cout <<endl;
}

这个小demo我放到了我的码云上，注释都挺清楚了，可以借鉴借鉴。
最后标定的结果为：

我所使用的realsense d435i，其640*480分辨率下的内参应该为：

可以看到，效果没有那么好。。。不知道什么原因呢。

ROS工具包标定

参考http://wiki.ros.org/camera_calibration/
这个工具包也是利用OPENCV的标定函数来封装的。
我这里运行：

rosrun camera_calibration cameralibrator.py --size 9x6 --square 0.108 image:=/camera/color

image是我自己的图像话题， —size我这里是9*6的棋盘，—square我没量，但这个不影响内参矩阵与畸变矩阵的结果。运行界面如下：

不断调整标定板或相机的角度，直到CALIBRATE按钮变绿，点击CALIBRATE按钮开始标定。
标定后会打印出标定结果。
在这里可以看到打印出的具体的参数说明。

  • D代表左右摄像头各自的畸变系数distortion_coefficients，为一乘五的矩阵；
  • K代表左右摄像头各自的代表焦距等信息的内部参数camera_matrix，为三乘三的矩阵，其格式如下：
  • R代表矫正矩阵rectification_matrix，一般为三乘三的单位阵，仅对双目相机有效，使左右极线平行；
  • P为外部世界坐标到像平面的投影矩阵projection_matrix，为三乘四的矩阵，其格式如下：

    左边的 3x3 部分是校正后图像的正常相机内在矩阵。它使用焦距 (fx’, fy’) 和主点 ((cx’, cy’) )，这些可能与 K 中的值不同。
    对于单目相机，第四列Tx = Ty = 0。对于双目相机，第四列 [Tx Ty 0] 是 与第二台摄像机的光学中心在第一台摄像机的帧中的位置有关。 我们假设 Tz = 0，所以两个相机都在同一个立体图像平面上。 第一个相机总是有 Tx = Ty = 0。对于水平立体对的右侧（第二个）相机，Ty = 0 和 Tx = -fx’ * B，其中 B 是相机之间的基线。

标定后得到的数据如下：

对比realsense d435i的内参，效果还是不错，所以还是推荐用ROS的标定工具包~

当然还有其它方法比如Kalibr，本土狗没接触那么多，哈哈
标定后使用undistort(test_image2, show_image, cameraMatrix, distCoeffs);函数来矫正图像去掉畸变。内参矩阵在深度还原时也会被用到，很重要。

over

~~结尾贪狼齐潇洒