三角测量

三角测量

使用对极约束估计了相机的运动，在得到运动之后，我们需要用相机的运动估计特征点的空间位置。在单目SLAM中，仅仅通过单张图像无法获得像素深度信息，需要通过三角测量来估计其深度。

上图中，O1P

和 O2P 理论上交与P点，但是由于噪声的存在，往往无法相交。可以通过最小二乘求解。设 x1,x2

为俩个特征点的归一化坐标，那他们满足：

s2x2=s1Rx1+t

 

由于我们已经知道R和t，想要求解的是俩个特征点的深度s1,s2

。当然这俩个深度可以分开求，若要求s1，则左乘 x∧

，得到

s2x∧2x2=0=s1x∧2Rx1+x∧2t

 

以上方程中，只需要对右边求方程即可，求出s1

，但是考虑到噪声的问题，估计的R，t

不一定为精确值，所以更常见的是求最小二乘，而不是求零解。

以上过程，可以通过代码来实现，以下只是主函数里面的代码，显示了主要流程：

int main ( int argc, char** argv )
{

    //-- 读取图像
    Mat img_1 =