基於opencv pnp的單目測距與姿態解算

單目測距與雙目測距一樣需要完成的第一步是相機的標定

推薦用matlab進行標定，標定的方法可以參看這個部落格http://blog.csdn.net/dreamharding/article/details/53700166

和https://blog.csdn.net/heroacool/article/details/51023921

在添加工具箱時注意選擇第二個add withsubfolders（和子檔案夾一起添加），否則容易導致用calib命令時打不開工具箱

標定相機需要注意的第二個問題是，由於使用的是外接usb網路攝影機其傳輸的像素大小由於usb頻寬節流設定為640*480，所以標定使用的圖片大小也需要縮放至640*480否則會導致標定出來的內參比實際使用時由於頻寬影響導致映像縮小的內參大數倍

標定完相機得到內參矩陣和畸變向量後就可以開始進行opencv的單目測距

單目測距使用的是sovelpnp函數，其第一個參數是輸入的三維點座標矩陣（單位是cm），注意此三維座標不能為隨意點（ps:網上有些的三維點寫的比較隨意），三維點需要與第二個參數imgPoints的點一一對應（imgPoints是輸入的映像二維點向量）

第三個參數是由上一步得到的相機內參矩陣（9*9）

第四個參數是相機的外參（1*5）

第五個參數是輸出的相機旋轉向量

第六個參數輸出的是相機的平移向量

通過旋轉向量和平移向量就可以得到相機座標系相對於全局座標系的旋轉參數與平移情況

關於全局座標系與相機座標系轉換的原理公式問題可以參看

http://blog.csdn.net/chenmohousuiyue/article/details/78157509?locationnum=9&fps=1

下面來發表一下我的理解

通過小孔成像的原理，根據相似三角形定理x*Z=f*X,其中的x是像素寬度，f是對應軸上的焦距，X是物體的實際長度，Z是物體到相機的距離

然後考慮到主點pinhole plane中的點的座標u（相對於（0，0）左上方的位移量），就可以得到x方向的像素座標，y方向同理可得

然後通過線性代數的知識把上述的式子寫成矩陣相乘

我這裡進行了逆向的推導證明式子的成立

可以發現其中的第一個矩陣是相機的內參矩陣。

上述式子得到的是相機座標系與映像座標系之間的關係，

由於相機座標系與全局座標系存在xyz軸的平移與yaw,pinch,row的旋轉，及全局座標系轉換到相機座標系需要乘上R(選擇矩陣)T（平移矩陣）

那麼可以得到像素座標系與全局座標系的關係就是在原有的像素座標繫到相機座標系的式子的基礎上乘以RT矩陣

即有：x =M*[R|t]*X，其中的M是相機的內參矩陣，R,T是全局座標轉換為相機座標的旋轉平移矩陣

到這一步後可以發現在等式x =M*[R|t]*X中我們已知x,X,M就可以得到R,T，所以這裡也可以反應出在相機標定中縮放映像為640*480的重要性，以及把像素點和全局座標點一一對應的重要性

這裡求R,T矩陣的過程就是使用opencv的sovelpnp輸入像素座標（及公式的x），全局座標(及公式的X)，輸入相機的內參矩陣和畸變矩陣（及公式的M）可以求得rvec和tvec及公式的（R,T矩陣）

在通過sovelpnp得到相機與全局座標系轉換關係的R,T矩陣後就是需要通過R,T矩陣求解出相機座標系相對於全局座標系的歐拉角關係和距離關係

旋轉矩陣的公式推導可以參看這個圖假使p點在原座標系中的點為（x，y），在旋轉角度o後的座標系中的座標為（x',y'）

可以得到數學公式為x'=xcoso-ysino,y'=xsino+ysino;

單個軸的旋轉矩陣為

旋轉矩陣R[3][3]

R[3][3]=R(row)*R(pitch)*R(yaw)得到的，可以得到總的

然後求解角度的公式為

其中rm就是那個3*3rvtc的矩陣，由sovelpnp求得,然後距離轉換公式我就不知道怎麼推倒就直接給我網上找的代碼了

最後的結果是測距精度為1cm左右