VINS-mono中的三角化方法（SVD）

1 引言

最近在跟着高博的《视觉SLAM十四讲》，自己手写一个双目里程计的一个项目，进行到了特征点的三角化部分时，发现其用的和VINS-mono一个方法。他并没有直接调用OpenCV中的现成函数，而是用了SVD分解的方法。所以心想写一个关于SVD分解进行特征点三角化的博客，加深一下印象。

2 三角化(14讲方法)

三角化又叫三角测量，本质是用相机的运动估计特征点的空间位置，发生在估计得到帧间运动之后（单目）。三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。由于双目本身就能得到视角不同的两帧图像，所以我们找到左右目相机的图像对应的特征点就可以直接进行三角测量；RGB-D相机就不用三角化了，直接通过物理手段得到深度值了。

如上图所示，如果我们知道了两帧图像中某对特征点的匹配关系，就能通过三角关系得出这对特征点对应的空间点坐标。
在高博的《视觉SLAM十四讲》中，是这样算的：

按照对极几何中的定义，设 $x1,x2$ 为两个特征点的归一化坐标，那么它们满足：

已知两帧的相对位姿关系$R，t$，想要求解的是两个特征点的深度$s_1和s_2$。他是两个值分开求的，比如：先算$s_2$，

求出$s_2，那么s_1$也就好求了。于是，就得到了两个帧下的点的深度，确定了它们的空间坐标。

3 三角化（SVD方法）

问题描述：已知左右两目相机的位姿以及特征点（相机归一化坐标）的匹配关系，求解特征点的世界坐标点。下面是具体推导过程：

我们接着把变换矩阵写成以下形式：

然后我们用到一个性质，如果两个向量可以写成$\vec{a}=k\vec{b}$，则$\vec{a}平行于\vec{b}$，而我们知道两个向量平行，那么他们的叉积就是$0$，所以就有了下面的式子：


这样一帧图像对应这么一个方程组，那么加上右目就是再有这样的两个式子拼成一个方程组（因为他们要求的未知数是一样的，都是世界坐标点），我们还发现，它实际上就是求解$Ax=0$的问题。

很容易得到$A$是一个$4\times 4$的矩阵，$p^w$是一个$4\times 1$的向量，解决这样一个$Ax=0$的问题，当然是可以用$SVD$分解的方法来做。

关于$SVD$分解的最优解求解问题，可以看我的另一篇博客。

4 两套代码示例

了解了方法之后再来看代码，
1、高博SVD三角化代码如下：

bool triangulation(const std::vector<SE3> &poses,
                   const std::vector<Vec3> points, Vec3 &pt_world) {
    MatXX A(2 * poses.size(), 4);//这里当然是4x4的系数阵
    VecX b(2 * poses.size());
    b.setZero();//b=0
    // 填充稀疏矩阵块
    for (size_t i = 0; i < poses.size(); ++i) {
        Mat34 m = poses[i].matrix3x4();
        A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
        A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
    }
    //进行SVD分解
    auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();//取前三维作为世界坐标（这里面包括了齐次向量归一化）
	// 这是保证最后一个奇异值相比上一个奇异值有较大的差距，才是有效解
    if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) {
        return true;
    }
    return false;// 解质量不好，放弃
}

2、VINS-mono的代码在这：

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	// 通过奇异值分解求解一个Ax = 0得到
	Matrix4d design_matrix = Matrix4d::Zero();
	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
	Vector4d triangulated_point;
	triangulated_point =
		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
	// 齐次向量归一化
	point_3d(0) = triangulated_point(0) / triangulated_point(3);
	point_3d(1) = triangulated_point(1) / triangulated_point(3);
	point_3d(2) = triangulated_point(2) / triangulated_point(3);
}

4 非常感谢您的阅读