三角化

路标点三角化

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

问题阐述

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

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

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

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

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

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

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

线性三角形法

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

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

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

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

目标

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

解法

$\mathrm{x}$是$\mathrm{V}$的最后一列,其中$A=UD{V}^{\mathrm{\top }}$是$A$的SVD分解

ORBSLAM中的实现

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

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

/** 给定投影矩阵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);
}