前言
本文通過迭代法解PNP問題，得到相機坐標系關(guān)于世界坐標系的旋轉(zhuǎn)矩陣R與平移矩陣T后，根據(jù)之前的文章《根據(jù)相機旋轉(zhuǎn)矩陣求解三個軸的旋轉(zhuǎn)角》獲得相機坐標系的三軸旋轉(zhuǎn)角，實現(xiàn)了對相機位姿的估計。知道相機在哪后，我們就可以通過兩張照片，計算出照片中某個點的高度，實現(xiàn)對環(huán)境的測量。
先看演示視頻：
原理簡介
相機位姿估計就是通過幾個已知坐標的特征點，以及他們在相機照片中的成像，求解出相機位于坐標系內(nèi)的坐標與旋轉(zhuǎn)角度，其核心問題就在于對PNP問題的求解，這部分本文不再啰嗦，參見本人之前的博客文章《相機位姿估計0：基本原理之如何解PNP問題》。本文中對pnp問題的求解直接調(diào)用了OpenCV的庫函數(shù)"solvePnP"，其函數(shù)原型為：
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> 。
