前言
本文通过迭代法解PNP问题，得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T后，根据之前的文章《根据相机旋转矩阵求解三个轴的旋转角》获得相机坐标系的三轴旋转角，实现了对相机位姿的估计。知道相机在哪后，我们就可以通过两张照片，计算出照片中某个点的高度，实现对环境的测量。
先看演示视频：
原理简介
相机位姿估计就是通过几个已知坐标的特征点，以及他们在相机照片中的成像，求解出相机位于坐标系内的坐标与旋转角度，其核心问题就在于对PNP问题的求解，这部分本文不再啰嗦，参见本人之前的博客文章《相机位姿估计0：基本原理之如何解PNP问题》。本文中对pnp问题的求解直接调用了OpenCV的库函数"solvePnP"，其函数原型为：
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
第一个输入objectPoints为特征点的世界坐标，坐标值需为float型，不能为double型，可以输入mat类型，也可以直接输入vector<point3f> 。
