三角测量

三角测量

使用对极约束估计了相机的运动，在得到运动之后，我们需要用相机的运动估计特征点的空间位置。在单目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 = imread ( "/home/hansry/Slam_Book/src/Test_trian/data/1.png", CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( "/home/hansry/Slam_Book/src/Test_trian/data/2.png", CV_LOAD_IMAGE_COLOR );

    vector keypoints_1, keypoints_2;
    vector matches;

    //对俩张图像，ORB特征检测，改进FAST关键点，BRIEF描述子，汉明距离（位数）进行匹配
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"< points;
    triangulation( keypoints_1, keypoints_2, matches, R, t, points );

    //-- 验证三角化点与特征点的重投影关系
    Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    for ( int i=0; i

通过三角测量及像素上的匹配点，我们可以求出路标即P点基于第一个相机坐标系的三维坐标点，然后通过对极约束求出的R，t可以求出在第二个相机坐标系上路标P点的三维坐标值。

讨论：三角平移是由平移得到的，有平移才会有对极几何约束的三角形。因此，纯旋转是无法使用三角测量的，对极约束将永远满足。在平移时，三角测量有不确定性，会引出三角测量的矛盾。

如上图所示，当平移很小时，像素上的不确定性将导致较大的深度不确定性，平移较大时，在相同的相机分辨率下，三角化测量将更精确。三角化测量的矛盾：平移增大，会导致匹配失效，平移太小，三角化精度不够。