在SLAM中。都会遇到对新提取的特征点计算其在3D空间中的坐标位置，也称作三角化。三角话可以从两个视角中进行恢复。当有多个视角的匹配点以后，也可以从多个视角中恢复3D点信息。

假设我们已经获得了两帧图像之间的精确的匹配点对，以及两帧图像之间的相对旋转和平移 [R|t] 。现在讨论如果计算匹配点对对应的3D点坐标。

输入： 两帧图像上的对应匹配点（通常为归一化坐标系），两帧图像之间的相对旋转和平移 [R|t]

输出： 特征点的3D坐标

设三维空间点 P 在世界坐标系中的坐标为 $X=[XYZ{]}^T$ ，在两个视角的投影点坐标为 $x_1,x_2$（归一化坐标系）。因此有如下关系：

针对双目的三角化问题，一共有四种不同的建模方式。

形式1：在等式左边同时乘$x_1$ 和$R{x}_2$

也是视觉 SLAM十四讲 中采用的求解方法

有：

这里为了借用了 《视觉SLAM十四讲》 中的标号求解方法。令 并带入式2，将上式写成矩阵形式，如下：：

这里使用了 克莱姆法则(Cramer’s Rule)
假设矩阵A是一个n×n矩阵，有方程AX=b 代求解，则有

而 $B_i$表示A的第i列被b向量替换后的矩阵。 根据克莱姆法则(Cramer’s Rule)，系数矩阵的行列式D及b为：

注意，由于 $f_{ref}^T$ 和 $f_2$ 都是3x1的向量，$f_{ref}^T{f}_2=f_2^T{f}_{ref}$ ,A_1=-A_2则：

这里可估计出两个深度值 d_ref 和 d_curr

参考代码：（输入的是其次坐标）

    Eigen::Vector3d f_ref,f_curr; //归一化坐标系点
    f_ref(0) = p1(0);  f_ref(1) =p1(1);  f_ref(2) = 1.0;
    f_curr(0) = p2(0); f_curr(1) = p2(1);  f_curr(2) = 1.0;
 
    Eigen::Vector3d f2 = R * f_curr; 
 
    Eigen::Vector2d b = Eigen::Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) );	// dot()会自动转换t与f_ref,保证t的列与f_ref的行相等，因此不用转置
    double A[4];	// A即上述的A0-A3
    A[0] = f_ref.dot (f_ref);
    A[2] = f_ref.dot (f2);
    A[1] = -A[2];
    A[3] = -f2.dot(f2);		// 此处应该是A[3] = - f_curr.dot ( f_curr );	
    // d 即系数矩阵D的行列式的值
    double d = A[0]*A[3]-A[1]*A[2];
    
    // 线性方程组求出的两个深度值：d_ref、d_curr
    Eigen::Vector2d lambdavec = 
        Eigen::Vector2d (  A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
                    -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;

	// 计算相机坐标系下的3D坐标
    if(lambdavec(0) < 0)  
	lambdavec(0) = -lambdavec(0);
    if(lambdavec(1) < 0)  
	lambdavec(1) = -lambdavec(1);

    Eigen::Vector3d xm = lambdavec(0) * f_ref;    // xm = s1*x1，即归一化相机坐标系转相机坐标系，s1即d_ref    
    Eigen::Vector3d xn = lambdavec(1) * f2 + t;	// xn = s2*R*x2 + t，xn为x2由当前坐标系转到参考坐标系下对应的3d坐标，s2即d_curr
    
    // 实质都是计算参考坐标系下的xm，此处是通过计算xm和xn平均值，减小误差
    Eigen::Vector3d d_esti = ( xm + xn ) / 2.0;  // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();   // 深度值
 
    std::cout << "CramersRule trianglede point is :" << d_esti(0) << " " << d_esti(1) << " " << d_esti(2) << std::endl<< std::endl;

形式2：直接变形

SVO 代码中使用的是该方法对特征点进行三角化

参考代码：（输入的是其次坐标）

//齐次坐标
	Eigen::Vector3d f_ref,f_curr;
	f_ref(0) = p1(0);  f_ref(1) =p1(1);  f_ref(2) = 1.0;
        f_curr(0) = p2(0); f_curr(1) = p2(1);  f_curr(2) = 1.0;
	
	Eigen::Matrix<double,3,2> A; 
	A << f_ref, -R * f_curr;

	Eigen::Matrix2d AtA = A.transpose() * A;

	if(AtA.determinant() < 0.000001)
		return ;

	Eigen::Vector2d depth2 =  -AtA.inverse()* A.transpose() * t;
	double depth = fabs(depth2(0));
	
	Eigen::Vector3d X = depth*f_ref;
	std::cout << "ATA trianglede point is :" << X(0) << " " << X(1) << " " << X(2) << std::endl<< std::endl;

形式3：等式两边同时叉乘x1

等式两边同时叉乘x1，后再变形为矩阵形式。

形式4：Direct Linear Transform

利用投影矩阵进行建模：(Multiple-View Geometry 第12章)。形式4中通常使用SVD求解方式，ORB-SLAM2、PTAM 中也是使用的该方式。

假设两帧图像对应的相机投影矩阵分为为P1,P2, 由投影关系有

对 x1 分别在等式左右两端分别叉乘 x1 向量

或者写为矩阵元素的形式：

由于第三个方程可以由前两个进行线性变换得到，因此每个视角可以提供两个约束条件，综合两个点

求解X相当于解一个线性最小二乘问题,对矩阵A进行SVD分解，A的最小奇异值对应的奇异向量就是方程的解，也就是空间点X的齐次坐标值。

最终X（第一视图坐标系下三维坐标）为

参考代码：（输入的是其次坐标）

	cv::KeyPoint kp1,kp2;
	cv::Mat mP1(3,4,CV_32F);
	cv::Mat mP2(3,4,CV_32F);
	kp1.pt.x = p1(0);kp1.pt.y = p1(1);
	kp2.pt.x = p2(0);kp2.pt.y = p2(1);
	for (int i = 0; i<3; i++)
		for (int j = 0; j<4; j++)
		{
			mP1.at<float>(i,j)=P1(i,j);
			mP2.at<float>(i,j)=P2(i,j);
		}

	cv::Mat mA(4,4,CV_32F);
	mA.row(0) = kp1.pt.x*mP1.row(2)-mP1.row(0);
	mA.row(1) = kp1.pt.y*mP1.row(2)-mP1.row(1);
	mA.row(2) = kp2.pt.x*mP2.row(2)-mP2.row(0);
	mA.row(3) = kp2.pt.y*mP2.row(2)-mP2.row(1);

	cv::Mat u,w,vt;
	cv::Mat x3D;
	cv::SVD::compute(mA,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
	x3D = vt.row(3).t();
	x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
	 
	std::cout << "SVD Opencv trianglede point is :" << x3D.at<float>(0) << " " << x3D.at<float>(1) << " " << x3D.at<float>(2) << std::endl<< std::endl;

完整的测试工程可以从我的Github [1]下载到
程序位于：slam_workspace/apps/triangulate_points/triangulate_points.cpp

老铁，来一波三连关注，收藏走起来。

参考资料

[1]
[2] https://blog.csdn.net/YunLaowang/article/details/99414762
[3] https://blog.csdn.net/YunLaowang/article/details/89640279
[4] https://blog.csdn.net/u011178262/article/details/86729887