14. 基于标定板的相机和车体之间外参标定思路及代码实现

1.相机外参标定的目的

自动驾驶汽车为什么一定要做标定，在我之前的文章里已经说明白了，这里做相机的外参标定也是让车知道相机观察到的信息在自己坐标系下应该是怎么样的。自动驾驶汽车可以说车身一周都配备了相机，相机就和人眼一样，相当于给车按了360度的眼睛。但是对于每路相机来说，都只看到自己有能力看到的一部分信息，如果我们把所有相机观测到的信息都转到车体坐标系上去，就能把车体周围一圈的信息拼在了一起，这就是标定的意义。如果没有这个标定好的外参，就不能从车的视角来观察到四周的场景到底是什么样的。

2. 基于标定板的标定思路

由于我们没有标定间的条件，那就继续延续之前的方案，使用标定板和全站仪配合。

具体操作就是：将车停好之后，把标定板放置在相机的前面，让相机能够观测到标定板，然后使用全站仪去测量标定板上的点的坐标以及四个车轮的坐标，这个是为了辅助我们计算得到标定板和车体之间的外参；相机观测到标定板可以检测出标定板上的角点信息，而我们又知道标定板上每个角点的物理坐标（z=0），这样就可以得到3D-2D的对应关系，就是PNP问题，通过求解PNP问题可以得到相机和标定板之间的外参；那么相机和车体之间的外参Tvc = Tcb * Tbs * Tsv得到。c:相机 b:标定板 s:全站仪 v:车体。

3. 不同外参求解方法的优缺点

求解PNP问题有很多种方法，比如P3P/EPNP/UPNP/优化迭代法/单应性分解等等。感兴趣的朋友可以去搜集资料去学习一下，这里直接列出各种方法的优缺点，根据这个表就可以确定我们什么场景下使用什么方法好，以及如何改进。

PNP经典求解方法	需要点对	共线/共面	需要注意的点
P3P	N>=3,实际是N>=4	用于计算的三个点不能共线	数据量多的时候不推荐，受噪声影响较大
EPNP	N>=4, 实际上N>=6更合适	无特殊要求	数据量大时，计算效率高；数据点共面时计算误差较大
DLT	N>=6	不能共面	SVD分解不是最终解，数据最好要进行归一化处理
单应性矩阵分解	N>=4	必须共面	所有点都在一个平面上
优化迭代	/	无特殊要求，但是比较依赖初始值	需要较好的初值，结合一些去除外点的策略使用

4. 标定思路

根据上述表格，我们可以根据自己的标定场景选择标定算法，比如我是基于标定板，标定板上的角点满足N>=4且所有点都在一个平面上，我就可以选择使用分解单应性矩阵的算法来求解外参Tcb。

4.1 计算单应性矩阵

应用背景：单应性变换是两个平面之间的几何变换，利用至少4对以上的同名点计算变换矩阵。
如果我们求解相机在两个视角拍的同一个标定板的图像之间的单应变换，那么这两个平面就是两张相机拍的图像。


我们求解到上面这个单应性矩阵H就可以分解出R，t来。

显然，对于我们的外参标定来说，我们这里的两个平面一个是标定板的物理平面，还有就是相机的图像平面。因为我们要求的是标定板和相机之间的单应性变换，而不是两个不同视角相机的单应性变换。

注意：虽然我们说两个平面分别是标定板的物理平面和相机的图像平面，但是为了单位统一，我们可以把图像平面的角点通过内参反投影到相机归一化平面，于是我们算法中的两个平面变为：
标定板的物理平面和相机归一化平面。

前面也说了，$x^{\prime }$和$x$都是齐次坐标，将像素坐标转换到相机归一化平面上正是这个目的。除此之外虽然我们标定板物理平面z=0,我们在计算H的时候要把它置为1。

求解过程在高博的十四讲也有，可以去看看。下面是求解过程：

这样我们可以构建这样一个方程$Ah=0$，如何求解这个方程之前也有聊过，可以看这个博客。

---

如果你想自己计算H矩阵可以参考以下代码：

// 计算单应性矩阵
int nPairSize = vCodedPairPt.size();
if (nPairSize < 4)
{
	cout << "匹配对数少于4对" << endl;
	return 0;
}
MatrixXd MatrixA(2 * nPairSize, 9);
double u1, v1, u2, v2;
int index = 0;
for (int i = 0; i < nPairSize; i++)
{
	u1 = vCodedPairPt[i].first->pt.dX;
	v1 = vCodedPairPt[i].first->pt.dY;
	u2 = vCodedPairPt[i].second->pt.dX;
	v2 = vCodedPairPt[i].second->pt.dY;
	index = i * 2;
	MatrixA(index, 0) = u1,     	 MatrixA(index, 1) = v1, 		  MatrixA(index, 2) = 1;
	MatrixA(index, 3) = 0,  		 MatrixA(index, 4) = 0, 		  MatrixA(index, 5) = 0;
	MatrixA(index, 6) = -u1*u2,      MatrixA(index, 7) = -v1*u2,      MatrixA(index, 8) = -u2;
	MatrixA(index + 1, 0) = 0,       MatrixA(index + 1, 1) = 0,       MatrixA(index + 1, 2) = 0;
	MatrixA(index + 1, 3) = u1,      MatrixA(index + 1, 4) = v1,      MatrixA(index + 1, 5) = 1;
	MatrixA(index + 1, 6) = -u1*v2,  MatrixA(index + 1, 7) = -v1*v2,  MatrixA(index + 1, 8) = -v2;
}
//cout << "MatrixA: " << MatrixA << endl;
JacobiSVD<MatrixXd> svd(MatrixA, ComputeThinU | ComputeThinV);
MatrixXd MatrixU;
MatrixU = svd.matrixV();
int nCols = MatrixU.cols();
VectorXd vSolution = MatrixU.col(nCols - 1);
Matrix3d MatrixH = Map<Matrix3d>(vSolution.data()).transpose();
cout << "MatrixH: " << MatrixH << endl;

其实OpenCV也有对应的接口，可以直接输入两个平面的点，输出单应性矩阵。不过这里要注意输入的点对是二维的。参考代码如下：

 std::vector<cv::Point2f> objects;
 for (auto pt : object_points) {
   cv::Point2f point(pt.x, pt.y);
   objects.push_back(point);
 }
 //     @param method Method used to compute a homography matrix. The following methods are
 //     possible:
 // -   **0** - a regular method using all the points, i.e., the least squares method
 // -   **RANSAC** - RANSAC-based robust method
 // -   **LMEDS** - Least-Median robust method
 // -   **RHO** - PROSAC-based robust method
 cv::Mat H = cv::findHomography(objects, image_points, cv::LMEDS);

4.2 从单应性矩阵中分解R，T

我们上一步求解出来了相机归一化平面和标定板平面之间的单应性矩阵之后，下一步就是从这个单应性矩阵中分解出R，T。
标定板坐标系与归一化平面之间的单应性矩阵$H$，其实质是如下形式：

由于标定板上角点z值是0，所以我们得到的$H$中不包含上面的$Mij$，而且等式里还有个缩放因子。因为我们要在里面分解出R，T，写成下面这样更直观：

所以我们要先计算出缩放因子，还得根据旋转矩阵的性质才保证H矩阵的前两列要是互相垂直的单位向量，$r3$就能通过$r1和r2$叉乘得到，$T=h3/{\lambda }_2$。

其实，此时的旋转矩阵仅是满足了列向量单位且正交，在行方向不一定满足。所以这时候得到的旋转矩阵还不是最终的矩阵。参考这篇博客，为了寻找一个接近与该矩阵并且完全满足旋转矩阵性质的矩阵，将该矩阵进行SVD分解, $R=U{V}^T$，得到我们最终的估计结果。
参考代码如下：

 double scale = 0.5 * (cv::norm(H.col(0)) + cv::norm(H.col(1)));
 H = H / scale;
 cv::Mat r1 = H.col(0).clone();
 cv::Mat r2 = H.col(1).clone();
 cv::Mat r3 = r1.cross(r2);  ///< r3
 cv::Mat R_R;
 cv::Mat trans;
 cv::hconcat(r1, r2, R_R);
 cv::hconcat(R_R, r3, R_R);
 trans = H.col(2).clone();

 cv::SVD svd(R_R);
 cv::Mat R_true = svd.u * svd.vt;

 Eigen::Matrix3d eigen_rot;
 Eigen::Vector3d eigen_t;
 cv::cv2eigen(R_true, eigen_rot);
 cv::cv2eigen(trans, eigen_t);

 extrinsic_.block<3, 3>(0, 0) = eigen_rot;
 extrinsic_.block<3, 1>(0, 3) = eigen_t;

5. 如果标定板上的点z坐标不为零该怎么办

其实基于标定板来标定都是把参考坐标系建立在标定板上也就是让z=0 ，但是有一种场景：比如你有一个标定间，你的标定板贴在墙上，你标定间里的所有标志物坐标都已知，而且都是基于标定间坐标系的，也就是已知标定板上角点的3D坐标，而且都是基于标定间的，所以他们的z不等于0。这时候我们就给你一组基于标定间的棋盘格角点3D坐标，结合图像中角点的2D检测坐标，来求这个单应性矩阵。
别忘了，前面求解单应性矩阵的时候要求两个面上角点坐标都是齐次坐标，也就是说我们提供大于等于4的点对，他们都是（x,y,1）的形式。如果标定板在标定间坐标系上z都不相同，就没法直接用。怎么办呢？

这种情况，肯定第一步就是将这组3D数据进行降维，他不是三维嘛，我们把他降为二维。

为什么能降维？因为这些点是共面的，所以我们完全可以用两个维度表示。如果我们把这些3D点构成一个矩阵A，求$A^{T}A$的特征值，可以得到他的第三维接近零。
降维嘛，很自然能想到PCA，SVD啥的，这块可以参考这篇博客。
代码实现参考如下：

    c::Mat image(image_points, true);
    image.convertTo(image, CV_64FC1);
    image = image.reshape(1).t();
    cv::Mat object(object_points, true);
    object.convertTo(object, CV_64FC1);
    object = object.reshape(1).t();
    cv::Mat mean, cov;
    cv::calcCovarMatrix(object, cov, mean, cv::COVAR_NORMAL | cv::COVAR_COLS);
    cv::SVD       svd(cov);
    cv::Mat R(svd.vt);  

    cv::Mat t = -R * mean;
    cv::Mat new_object(object.size(), CV_64FC1);
    new_object = R * object + t * cv::Mat::ones(1, image.cols, CV_64FC1);

上面操作的本质其实就是将标定板的平面和世界坐标线的xoy平面对齐了，旋转矩阵其实就是由数据组成矩阵的协方差矩阵的特征向量组成的，由于他第三个特征值特别小，所以原来的点第三维经过旋转后都变成0了。

注意：这里面包含了一个欧式变换，将标定板的坐标转到一个世界平面上去了，后面我们求解出来的Tcb其实是相机和这个世界平面之间的H，分解出R，T之后要乘以这个欧式变换的逆变换回去才是真正的Tcb。

6. 欢迎来一起读书，远离焦虑