三角化

发表于 分类于
三角化的数理流程以及工程实现

路标点三角化

这里原理部分主要是参考多视图几何低十一章,结构计算,代码部分主要参考ORBSLAM中的实现

问题阐述

在已知两视图之间的变换关系-基本矩阵F,求解三维点X

假设测量点是\(\mathrm{x}\)和\(\mathrm{x}^{\prime}\),\(\mathbf{x} \leftrightarrow \mathbf{x}^{\prime}\),如果没有误差,那么存在一个三维点\(\mathbf{X}\)满足\(\mathbf{x}=\mathrm{P} \mathbf{X}, \mathbf{x}^{\prime}=\mathrm{P}^{\prime} \mathbf{X}\).其中P表示摄像机矩阵

定义\(\tau\)是对应点\(\mathbf{x} \leftrightarrow \mathbf{x}^{\prime}\)以及一对相机矩阵\(\mathrm{P}\)和\(\mathrm{P}^{\prime}\)来计算空间点\(\mathbf{X}\)的一种三角形法,有 \[ \mathbf{X}=\tau\left(\mathbf{x}, \mathbf{x}^{\prime}, \mathbf{P}, \mathbf{P}^{\prime}\right) \] 该三角形称为在变换H下是不变的,如果 \[ \tau\left(\mathbf{x}, \mathbf{x}^{\prime}, \mathbf{P}, \mathbf{P}^{\prime}\right)=\mathbf{H}^{-1} \tau\left(\mathbf{x}, \mathbf{x}^{\prime}, \mathrm{PH}^{-1}, \mathrm{P}^{\prime} \mathrm{H}^{-1}\right) \] 这也就是说,用了变换之后的相机矩阵也可以得到相同的三维空间点,也就是使用这种方法是XX变换不变的.

###　为什么用二维误差而不是三维

对于射影重构而言，在三维射影空间\(\mathbb{P}^{3}\)对误差进行最小化是不合适的．因为像距离和垂直等概念在射影几何下无效．如果使用这种三维空间下的误差来计算的话，这种方法并不是射影不变的

真正的射影不变的方法需要在二维上进行，估计一个三维点\(\hat{\mathrm{X}}\)，其满足摄像机几何，所以他的投影便是 \[ \hat{\mathbf{x}}=\mathrm{P} \hat{\mathbf{X}} \quad \hat{\mathbf{x}}^{\prime}=\mathrm{P}^{\prime} \hat{\mathbf{X}} \] 这么做的目的就是，使用测量的\(\mathrm{x}\)和\(\mathrm{x}^{\prime}\),来估计\(\hat{\mathrm{X}}\)

在噪声的影响下，最大似然估计由最小化重投影误差－－\(\hat{\mathrm{X}}\)的投影到图像测量点的距离得到

线性三角形法

线性三角形法就是一种DLT的方式,在每一幅图上都有测量\(\mathbf{x}=\mathrm{P} \mathbf{X}, \mathbf{x}^{\prime}=\mathrm{P}^{\prime} \mathbf{X}\),且这些方程可以组合成\(\mathrm{AX}=0\)的方式,其是关于\(\mathrm{X}\)的线性方程.

因为有\(\mathbf{x}=\mathrm{P} \mathbf{X}\),所以可以得到叉乘的形式,也就是\(\mathbf{x} \times(\mathrm{P} \mathbf{X})=\mathbf{0}\),展开可以得到 \[ \begin{aligned} x\left(\mathbf{p}^{3 \top} \mathbf{X}\right)-\left(\mathbf{p}^{1 \top} \mathbf{X}\right) &=0 \\ y\left(\mathbf{p}^{3 \top} \mathbf{X}\right)-\left(\mathbf{p}^{2 \top} \mathbf{X}\right) &=0 \\ x\left(\mathbf{p}^{2 \top} \mathbf{X}\right)-y\left(\mathbf{p}^{1 \top} \mathbf{X}\right) &=0 \end{aligned} \] 其中\(\mathbf{p}^{i \top}\)是相机矩阵\(\mathrm{P}\)的行,所以\(\mathrm{AX}=0\)方程使用两个点可以获得以下的A矩阵 \[ A=\left[\begin{array}{c} x \mathbf{p}^{3 \top}-\mathbf{p}^{1 \top} \\ y \mathbf{p}^{3 \top}-\mathbf{p}^{2 \top} \\ x^{\prime} \mathbf{p}^{\prime 3 T}-\mathbf{p}^{\prime 1 \top} \\ y^{\prime} \mathbf{p}^{\prime 3 \top}-\mathbf{p}^{\prime 2 \top} \end{array}\right] \] 也就是说,每一组像点可以获得4个方程,这是一个冗余的方程组,因此可以使用齐次的方式(DLT)来进行求解

###　齐次方程组的最小二乘解

考虑到方程\(\mathrm{AX}=0\)，同时该方程方程数目大于未知量个数，因此是一个超定方程组，如果ｘ是方程组一个解，那么kx这些也是属于方程的解,也就是存在解空间.具体解法如下

目标

给定一个行数不少于列数的矩阵A,求最小化\(\|\mathrm{Ax}\|\)并满足\(\|\mathrm{x}\|=1\)的x

解法

\(\mathrm{x}\)是\(\mathrm{V}\)的最后一列,其中\(A=U D V^{\top}\)是\(A\)的SVD分解

ORBSLAM中的实现

ORBSLAM中关于三角化点的构造在Initializer中,

其实现和之前原理中的是一模一样的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
/** 给定投影矩阵P1,P2和图像上的匹配特征点点kp1,kp2，从而计算三维点坐标
 * @brief 
 * 
 * @param[in] kp1               特征点, in reference frame
 * @param[in] kp2               特征点, in current frame
 * @param[in] P1                投影矩阵P1
 * @param[in] P2                投影矩阵P2
 * @param[in & out] x3D         计算的三维点
 */
void Initializer::Triangulate(
    const cv::KeyPoint &kp1,    //特征点, in reference frame
    const cv::KeyPoint &kp2,    //特征点, in current frame
    const cv::Mat &P1,          //投影矩阵P1
    const cv::Mat &P2,          //投影矩阵P2
    cv::Mat &x3D)               //三维点
{
    // 原理
    // Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
    // x' = P'X  x = PX
    // 它们都属于 x = aPX模型
    //                         |X|
    // |x|     |p1 p2  p3  p4 ||Y|     |x|    |--p0--||.|
    // |y| = a |p5 p6  p7  p8 ||Z| ===>|y| = a|--p1--||X|
    // |z|     |p9 p10 p11 p12||1|     |z|    |--p2--||.|
    // 采用DLT的方法：x叉乘PX = 0
    // |yp2 -  p1|     |0|
    // |p0 -  xp2| X = |0|
    // |xp1 - yp0|     |0|
    // 两个点:
    // |yp2   -  p1  |     |0|
    // |p0    -  xp2 | X = |0| ===> AX = 0
    // |y'p2' -  p1' |     |0|
    // |p0'   - x'p2'|     |0|
    // 变成程序中的形式：
    // |xp2  - p0 |     |0|
    // |yp2  - p1 | X = |0| ===> AX = 0
    // |x'p2'- p0'|     |0|
    // |y'p2'- p1'|     |0|
    // 然后就组成了一个四元一次正定方程组，SVD求解，右奇异矩阵的最后一行就是最终的解.

	//这个就是上面注释中的矩阵A
    cv::Mat A(4,4,CV_32F);

	//构造参数矩阵A
    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

	//奇异值分解的结果
    cv::Mat u,w,vt;
	//对系数矩阵A进行奇异值分解
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
	//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定
	//别忘了我们更习惯用列向量来表示一个点的空间坐标
    x3D = vt.row(3).t();
	//为了符合齐次坐标的形式，使最后一维为1
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}