视觉SLAM实践入门——（12）三角测量

前面通过对极约束估计了相机的 R，t，这一节通过三角测量可以恢复深度值，得到特征点的空间位置（估计值）

利用opencv进行三角测量的步骤：

1、定义旋转矩阵和平移向量组成的增广矩阵

2、计算特征点在两个坐标系下的归一化坐标（取前两维）

3、调用triangulatePoints，得到空间点的齐次坐标

4、归一化处理，取前三维作为空间点的非齐次坐标

 

需要注意的是：三角测量是由平移得到的，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，还要关心三角测量的不确定性，这会引出一个三角测量的矛盾

如果特征点运动一个像素 δx，使得视线角变化了一个角度 δθ，那么测量到深度值将有 δd 的变化。从几何关系可以看到，当 t 较大时，δd 将明显变小，这说明平移较大时，在同样的相机分辨率下，三角化测量将更精确

要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，提高计算成本

另一方式是使平移量增大。但是，平移量增大会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来，比如反射光发生变化等等。外观变化会使得特征提取与匹配变得困难

因此出现了三角化的矛盾：平移增大，可能导致匹配失效；平移减小，三角化精度不够

 

 

 

#include <iostream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
using namespace cv;

void OrbFeaturesMatch(const Mat &img1, const Mat &img2,
			vector<KeyPoint> &kp1, vector<KeyPoint> &kp2,
			vector<DMatch> &matches)
{
	Mat desc1, desc2;
	
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> desc = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-