cv::triangulatePoints函数参数

两个相机位姿和特征点在两个相机坐标系下的坐标，输出三角化后的特征点的3D坐标。

注意：输出的3D坐标是齐次坐标，共四个维度，因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz。这个坐标是在相机坐标系下的坐标，以输入的两个相机位姿所在的坐标系为准。

在主函数中，通过把3D坐标重投影到两个相机的归一化平面上，从而计算重投影误差。因此需要再次对xyz坐标同时除以z，以得到归一化平面上的坐标。

1.对极几何

从2张图片中,得到若干个配对好的2d特征点,就可以运用对极几何来恢复出两帧之间的运动.

设P的空间坐标为:

两个像素点p1,p2的像素坐标为:

K为相机内参,R,t为图像1到图像2的旋转矩阵和平移矩阵.

相机姿态估计问题变成以下两步:

• 根据配对点的像素位置求出R或者F
• 根据E或F求出R,t

2.本质矩阵

根据本质矩阵

定义,这是一个3*3的矩阵,经典是使用8点法来求解.求解出E后,可通过奇异值分解得到相机的运动R和t.

注意:求出的E和t具有尺度一致性,通常把t进行归一化.

3.尺度不确定性

对t的长度归一化,直接导致单目视觉的尺度不确定性.解决办法可以通过SLAM的初始化来解决,初始时,使机器人平移一段距离,然后以此距离作为平移的单位.初始化之后,就可以使用3D-2D来计算相机运动了

工程中,通常匹配的点比较多,这时可以通过构造最小二乘法来进行求解E,但是由于存在误匹配的情况,所以更多的是使用随机采样一致性(RANSAC)来求解

4.三角测距来测量深度

根据对极几何的定义,x1,x2

 

5.验证

通过方程 对第一帧3D坐标进行重投影，得到第二帧3D坐标
代码为：

Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t;

 

 通过4中计算得到的第二帧3D点，除以z得到第二帧相机归一化坐标

pt2_trans /= pt2_trans.at<double>(2,0);