SLAM之单目视觉里程计

基础知识

旋转矩阵和位移矩阵

建议参考计算机图形学教材，学习“图形变换”章节。重点掌握以下概念：

旋转矩阵
平移向量
齐次坐标
变换矩阵（旋转矩阵与平移向量的结合）

参考：

三个坐标系

图像坐标系：一般来说，图像坐标系包括像素坐标系以及物理坐标系。

图像坐标系

对于图像直角坐标系上的坐标$(u,v)$，分别能从像素的行数和列数得到，叫做像素坐标系。为表示像素点在图像中的位置我们需要借助物理坐标系，以图像中某一点$O_{1}$当作原点建立坐标系，其中$x$轴，$y$轴分别与像素坐标系的横、竖轴平行，方向一致。其中$(x, y)$坐标单位为毫米，$O_{1}$在$(u, v)$坐标系下的坐标为$\left(u_{0}, v_{0}\right)$，$d_{x}$和$d_y$表示每个像素点在$x$,$y$轴上的尺寸大小，则有： $$ \left{\begin{array}{l}{u=\frac{x}{d x}+u_{0}} \ {v=\frac{y}{d y}+v_{0}}\end{array}\right. $$ 即： $$ \left[ \begin{array}{c}{u} \ {v} \ {1}\end{array}\right]=\left[ \begin{array}{ccc}{\frac{1}{d x}} & {0} & {u_{0}} \ {0} & {\frac{1}{d y}} & {v_{0}} \ {0} & {0} & {1}\end{array}\right] \left[ \begin{array}{l}{x} \ {y} \ {1}\end{array}\right] $$ 相机坐标系：相机坐标系原点位于光心。如图所示，相机光心为点$O$，$Z_{c}$与像平面垂直，为相机的光轴，$X_{c}, Y_{c}$分别与$x,y$轴相平行，焦距大小为线段$O O_{1}$的长度，则将$O-X_{c} Y_{c} Z_{c}$坐标系称作相机坐标系。

相机坐标系

世界坐标系:世界坐标系可以对场景的中的任何物体进行描述，同时能表示出相机坐标。如上图中，将坐标系$O_{w}-X_{w} Y_{w} Z_{w}$称为世界坐标系，其与相机坐标系的相互变换可以通过$3 \times 1$的平移向量$t$以及$3\times 3$的旋转矩阵$R$来表示，设点$P$在世界坐标系中的齐次坐标为$\left(X_{w}, Y_{w}, Z_{w}, 1\right)^{T}$，在相机坐标系中的齐次坐标为$\left(X_{c}, Y_{c}, Z_{c}, 1\right)^{T}$，那么二者的转换关系为： $$ \left[ \begin{array}{c}{X_{c}} \ {Y_{c}} \ {Z_{c}} \ {1}\end{array}\right]=\left[ \begin{array}{cc}{R} & {t} \ {0^{T}} & {1}\end{array}\right] \left[ \begin{array}{c}{X_{w}} \ {Y_{w}} \ {Z_{w}} \ {1}\end{array}\right]=M_{1} \left[ \begin{array}{c}{X_{w}} \ {Y_{w}} \ {Z_{w}} \ {1}\end{array}\right] $$

针孔相机模型

针孔相机模型是将空间坐标映射到图像点的模型。

请阅读: 视觉SLAM十四讲 5.1.1

对于使用的数据集，一般会提供拍摄照片所用的相机的内参和畸变系数。如果使用自己的相机，则需要采用张正友标定法进行相机标定，以获取相机内参及畸变系数。

最终有： $$ Z \boldsymbol{P}{u v}=Z \left[ \begin{array}{c}{u} \ {v} \ {1}\end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}{w}+\boldsymbol{t}\right)=\boldsymbol{K} T \boldsymbol{P}_{w} $$

其中$Z$为空间点三维坐标中的$Z$坐标值，$P_{uv}$代表空间点投影到像平面，在像平面上的坐标，$K$为相机内参，$R$为相机外参中的旋转矩阵，$t$为相机外参中的位移向量，$P_w$为空间点在世界坐标系下的三维坐标。其中： $$ K=\left( \begin{array}{ccc}{f_{x}} & {0} & {c_{x}} \ {0} & {f_{y}} & {c_{y}} \ {0} & {0} & {1}\end{array}\right) $$

单目视觉里程计

单目视觉里程计主要需要了解对极约束、三角化和PnP。

对极约束

对极约束用于通过二维点之间的匹配关系，求解相邻两张图像之间的运动。关于对极约束的内容，请阅读视觉SLAM十四讲7.3，7.4。

使用对极约束求解本质矩阵可以通过调用opencv中的findEssentialMat函数完成。

//函数原型
Mat findEssentialMat( 
    InputArray points1, 
    InputArray points2,
    InputArray cameraMatrix, 
    int method = RANSAC,
    double prob = 0.999, 
    double threshold = 1.0,
    OutputArray mask = noArray());

//示例调用
using namespace cv;
Mat E = findEssentialMat(p1, p2, M);

其中：

points1与points2是vector<Point2f>类型，两vector的size相同，对应位置上的Point2f互相匹配。
cameraMatrix是相机内参矩阵
method是计算方法，RANSAC是openCV中被define的常量，表示采用RANSAC方法进行本征矩阵计算
prob是估计矩阵正确的可信度
参数threshold用于RANSAC的参数。它是从点到极线的最大距离（以像素为单位），超出此点时，该点被视为异常值，不用于计算最终的基本矩阵。根据点定位精度，图像分辨率和图像噪声的不同，可将其设置为1-3。
参数mask输出N个元素的数组，其中每个元素对于异常值设置为0，对其他点设置为1。

求解出本征矩阵E后，调用recoverPose求解旋转矩阵和位移向量。

//函数原型
int recoverPose(Mat E,
                InputArray points1,
                InputArray points2,
                InputArray cameraMatrix,
                OutputArray R,
                OutputArray t,
                OutputArray mask = noArray()); 
//示例调用
using namespace cv;
int pass_count = recoverPose(E,p1, p2, M, R, t,mask);

其中：

E是经过findEssentialMat求解出的本征矩阵
points1，points2，cameraMatrix，mask与findEssentialMat函数中的意义相同
R是求解出的旋转矩阵
t是求解出的位移向量

R和t组成变换矩阵T，这里的T是是第一张图到第二张图的坐标变换矩阵。或者可以理解为第一张图相机相对于相机初始位置的变换矩阵是X。因为第一张图相机所处的位置是初始位置，所以有：$X = eye(4)$，其中$eye(4)$是四阶单位矩阵。那么T左乘X是第二个图的相机的坐标变换矩阵。

对极约束可以求解出两张图像之间的相机变换矩阵，但是它是2D和2D之间求解的变换矩阵，t的尺度无法确定，t可能是1米，可能是1cm。

三角化

void reconstruct(Mat& K, Mat& R1, Mat& T1, Mat& R2, Mat& T2, vector<Point2f>& p1, vector<Point2f>& p2, vector<Point3f>& structure)
{
	//两个相机的投影矩阵[R T]，triangulatePoints只支持float型
	Mat proj1(3, 4, CV_32FC1);
	Mat proj2(3, 4, CV_32FC1);

	R1.convertTo(proj1(Range(0, 3), Range(0, 3)), CV_32FC1);
	T1.convertTo(proj1.col(3), CV_32FC1);

	R2.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
	T2.convertTo(proj2.col(3), CV_32FC1);

	Mat fK;
	K.convertTo(fK, CV_32FC1);
	proj1 = fK*proj1;
	proj2 = fK*proj2;
	//三角重建
	Mat s;
	triangulatePoints(proj1, proj2, p1, p2, s);

	structure.clear();
	structure.reserve(s.cols);
	for (int i = 0; i < s.cols; ++i)
	{
		Mat_<float> col = s.col(i);
		col /= col(3);  //齐次坐标，需要除以最后一个元素才是真正的坐标值
		structure.push_back(Point3f(col(0), col(1), col(2)));
	}
}

三角化在得到两个相机的旋转矩阵、位移向量后，可以根据匹配的特征点进行三角化，最终能够求得特征点对应的空间点云的坐标。

PnP

PnP在得知了对应的空间点的三维坐标和新加入的图像中的特征点的二维坐标之后，可以求解新加入的图像的相机的旋转矩阵和位移向量。

参考opencv中solvePnPRansac函数求解相机位姿

//函数原型
bool solvePnPRansac(InputArray objectPoints,
                    InputArray imagePoints,
                    InputArray cameraMatrix,
                    InputArray dist，
                    OutputArray rvec,
                    OutputArray tvec,
                    ...);
//调用示例
solvePnPRansac(object_points, image_points, K, noArray(), r, T);

其中：

objectPoints为三维坐标，imagePoints为二维坐标，对应位置的坐标互相匹配。
cameraMatrix为相机内参。
dist为畸变参数矩阵。
rvec为求解出的相机旋转向量，不是旋转矩阵。可经过罗德里格斯变换获取旋转矩阵。

1	Rodrigues(r, R);

tvec为求解出的相机位移向量。

这里的objectPoints为原有空间点云三维坐标。imagePoints为新加入的一张图像中的特征点的图像坐标。假设刚开始运行程序时候，点云$C_{12}$由前两张图像的相机的旋转矩阵、位移向量和匹配的特征点经过三角化求解出来。

上述过程涉及到的前两张图像中第一张图像的相机旋转矩阵为三阶单位矩阵，位移向量为0，即第一张图像的相机处于起始位置；第二张图像的旋转矩阵和位移向量通过对极几何被求解出来。

进行求解PnP需要构建已有空间点云三维坐标和新加入图像中的特征点二维坐标的匹配关系。那么这个关系是怎么建立的呢？是在图一、图二进行三角化生成空间点云时候，会生成空间点云中点与图二中特征点的匹配关系。图二与图三进行匹配，可获得图二与图三中特征点的匹配关系。由此可获得图三中特征点与原有空间点云中点的匹配关系。

程序经过一次PnP，求解出第三张图像的相机的旋转矩阵和位移矩阵后，可以使用第三张图像的相机的旋转矩阵和位移向量与第二张图像的旋转矩阵和位移向量进行三角化，生成第二张图像和第三张图像共同构建的空间点云$C_{23}$。

之后，空间点云可以进行点的添加和旧点的消除，每次新加入的一张图像的相机位姿可以通过PnP求解出来。

参考资料

【1】《视觉SLAM十四讲》 —— 高翔

【2】葛均强. 基于无人机航拍图像序列的三维重建研究与实现[D]. 电子科技大学,2015.

【3】线性代数的本质——3blue1brown