關鍵詞：相機位姿估計，單目尺寸測量，環境探知

用途：基於相機的環境測量，SLAM，單目尺寸測量

文章型別：原理說明、Demo展示

@Author：VShawn

@Date:2016-11-28

@Lab: [email protected]

前言

早就寫好了....不過doc放在膝上型電腦裡，平時一直都在用桌上型電腦，所以拖到現在才發:(

寫了這麼多篇關於位姿估計的部落格後，終於要寫一篇有點用的東西了：本文將展示位姿估計的一種應用，即通過單目相機對環境進行測量。簡單來說，本文的工作就是利用下面的兩幅圖，在已知P1、P2、P3、P4四點世界座標的情況下，計算出P5的世界座標。

該研究的應用範圍很廣，例如對某建築群，可以通過設定幾個已知的標誌點（世界座標已確定），用本文的方法將建築的各個角的世界座標求出來，從而測量出建築的高度，建築間的距離，乃至將整個建築群的環境重構出來。又或者在某個露天貨場，設定好標誌點後只需要無人機飛一圈，就能知道貨場中貨物的體積有多少，從而安排貨運計劃。總之該項應用的前景很大，配合目前很火的無人機應用，可以為生產、研究帶來不少的便利。最後，本文基於前幾篇文章的結果，建議沒有看過我部落格的讀者先讀讀前面的幾篇原理介紹。

原理

在一開始，先設待求點為P。

根據兩條直線確定一個點的原理，在二維平面中只要知道兩條相交直線的方程，就可以解出它們的交點座標。現在假設我們是在二維平面中拍照

根據文章《相機位姿估計1：根據四個特徵點估計相機姿態》的內容，我們根據P1、P2、P3、P4四點的空間座標，可以估計出兩次拍照的相機位姿Oc1與Oc2，也就知道了相機的座標Oc1與Oc2。那麼將Oc1與P，Oc2與P聯成直線（如上圖的橙色線），則可以獲得兩條直線方程，組成方程組求解得到它們的交點，即為待求點P的座標。

到三維空間中，原理跟二維是一樣的，只是兩條直線從二維空間升到了三維空間成為了兩條空間。通過解PNP求出了相機兩次拍攝的空間位置Oc1、Oc2，在根據P在影象中的座標，可以知道P點在空間中位於相機的哪個方向（將二維影象中的P點用公式對映到三維空間中，需要使用到內參數與外引數矩陣

1.求出P點的相機座標系座標Pc

關於P點如何從二維對映到三維，參考上圖，Oc的座標通過解PNP已經求出，待求點P在影象中的畫素座標為(u,v)。根據《影象座標系-相機座標系-世界座標系的關係》（由於懶癌，還沒寫）可以套公式求出P在相機座標系中的座標Pc（也就是上圖中的Pc點）。具體的轉換公式如下，式中F為相機鏡頭的焦距(mm)，u、v為點的畫素座標，其餘為相機內參數。

程式碼如下：

程式碼中使用本人封裝好的解PNP問題類解決PNP問題，具體使用方法參見《OpenCV:solvePnP二次封裝與效能測試》。

    PNPSolver p4psolver1;
    //初始化相機引數
    p4psolver1.SetCameraMatrix(fx, fy, u0, v0);
    //設定畸變引數
    p4psolver1.SetDistortionCoefficients(k1, k2, p1, p2, k3);

    p4psolver1.Points3D.push_back(cv::Point3f(0, 0, 0));        //P1三維座標的單位是毫米
    p4psolver1.Points3D.push_back(cv::Point3f(0, 200, 0));        //P2
    p4psolver1.Points3D.push_back(cv::Point3f(150, 0, 0));        //P3
    p4psolver1.Points3D.push_back(cv::Point3f(150, 200, 0));    //P4
    //p4psolver1.Points3D.push_back(cv::Point3f(0, 100, 105));    //P5

    cout << "特徵點世界座標 = " << endl << p4psolver1.Points3D << endl << endl << endl;

    //求出圖一中幾個特徵點與待求點P的座標
    //cv::Mat img1 = cv::imread("1.jpg");
    p4psolver1.Points2D.push_back(cv::Point2f(2985, 1688));    //P1
    p4psolver1.Points2D.push_back(cv::Point2f(5081, 1690));    //P2
    p4psolver1.Points2D.push_back(cv::Point2f(2997, 2797));    //P3
    p4psolver1.Points2D.push_back(cv::Point2f(5544, 2757));    //P4
    //p4psolver1.Points2D.push_back(cv::Point2f(4148, 673));    //P5

    cout << "圖一中特徵點座標 = " << endl << p4psolver1.Points2D << endl;

    cv::Point2f point2find1_IF = cv::Point2f(4149, 671);//圖1中待求點P的影象座標系座標

    if (p4psolver1.Solve(PNPSolver::METHOD::CV_P3P) != 0)
        return -1;

    cout << "圖一中相機位姿" << endl << "Oc座標=" << p4psolver1.Position_OcInW << "      相機旋轉=" << p4psolver1.Theta_W2C << endl;

    //將P投射到相機座標系，再經過反旋轉求出向量OcP，最終獲得圖1中，直線OcP上的兩個點座標，確定了直線的方程
    cv::Point3f point2find1_CF = p4psolver1.ImageFrame2CameraFrame(point2find1_IF, 350);//待求點P在圖一狀態下的相機座標系座標，輸入引數350表示將P投影到350mm外的相機成像平面

2.求出P點在世界座標系中的方向向量

此時我們得到了Pc(xc,yc,zc)，但這個點座標是在相機座標系中的，而我們需要知道的其實是P點在世界座標系中對應的座標Pw(xw,yw,cw)。為了將Pc轉為Pw，需要使用到解PNP求位姿時得到的三個尤拉角。我們知道相機座標系C按照z軸、y軸、x軸的順序旋轉以上角度後將與世界座標系W完全平行（詳見《子座標系C在父座標系W中的旋轉問題》），在這三次旋轉中Pc顯然是跟著座標系旋轉的，其在世界系W中的位置會隨著改變。為了抵消旋轉對P點的影響，保證C系旋轉後P點依然保持在世界座標系W原本的位置，需要對Pc進行三次反向旋轉，旋轉後得到點Pc在相機座標系C中新的座標值記為Pc'，Pc'的值等於世界座標系中向量OP的值。那麼Pc'的值+ Oc的世界座標值=P點的世界座標Pw。

程式碼如下（程式碼中變數接上一段程式碼）：

    double Oc1P_x1 = point2find1_CF.x;//待求點P的相機座標系x座標
    double Oc1P_y1 = point2find1_CF.y; //待求點P的相機座標系y座標
    double Oc1P_z1 = point2find1_CF.z; //待求點P的相機座標系z座標
    //進行三次反向旋轉，得到世界座標系中向量OcP的值，也就是方向向量
    PNPSolver::CodeRotateByZ(Oc1P_x1, Oc1P_y1, p4psolver1.Theta_W2C.z, Oc1P_x1, Oc1P_y1);
    PNPSolver::CodeRotateByY(Oc1P_x1, Oc1P_z1, p4psolver1.Theta_W2C.y, Oc1P_x1, Oc1P_z1);
    PNPSolver::CodeRotateByX(Oc1P_y1, Oc1P_z1, p4psolver1.Theta_W2C.x, Oc1P_y1, Oc1P_z1);
    //兩點確定一條直線，a1為Oc的世界座標，a2為P的世界座標Pw
    cv::Point3f a1(p4psolver1.Position_OcInW.x, p4psolver1.Position_OcInW.y, p4psolver1.Position_OcInW.z);
    cv::Point3f a2(p4psolver1.Position_OcInW.x + Oc1P_x1, p4psolver1.Position_OcInW.y + Oc1P_y1, p4psolver1.Position_OcInW.z + Oc1P_z1);

 上面的程式碼中獲得了一條射線A的兩個端點，其中a1為相機的世界座標系座標，a2為求出的P點對映到世界座標系時的方向向量+相機的世界座標系座標

3.最後，根據兩幅圖得到的兩條直線，計算出P點的世界座標

對另外一幅圖也進行1、2的操作，得到點b1，b2。於是獲得兩條直線A、B，求出兩條直線A與B的交點，大功告成。然而在現實中，由於誤差的存在，A與B相交的可能性幾乎不存在，因此在計算時，應該求他們之間的最近點座標。

根據文章《求空間內兩條直線的最近距離以及最近點的座標(C++)》中給出的GetDistanceOf2linesIn3D類，可以求出兩條直線的交點或者說兩條直線的最近點座標。

程式碼：

    /*************************求出P的座標**************************/
    //現在我們獲得了關於點P的兩條直線a1a2與b1b2
    //於是兩直線的交點便是點P的位置
    //但由於存在測量誤差，兩條直線不可能是重合的，於是退而求其次
    //求出兩條直線最近的點，就是P所在的位置了。

    GetDistanceOf2linesIn3D g;//初始化
    g.SetLineA(a1.x, a1.y, a1.z, a2.x, a2.y, a2.z);//輸入直線A上的兩個點座標
    g.SetLineB(b1.x, b1.y, b1.z, b2.x, b2.y, b2.z);//輸入直線B上的兩個點座標
    g.GetDistance();//計算距離
    double d = g.distance;//獲得距離
    //點PonA與PonB分別為直線A、B上最接近的點，他們的中點就是P的座標
    double x = (g.PonA_x + g.PonB_x) / 2;
    double y = (g.PonA_y + g.PonB_y) / 2;
    double z = (g.PonA_z + g.PonB_z) / 2;

    cout << endl << "-------------------------------------------------------------" << endl;
    cout << "解得P世界座標 = (" << x << "," << y << "," << z << ")" << endl;

結果

最後解得P點座標為：

P的實際座標為(5,100,105)，計算結果的誤差在1mm左右，考慮到繪圖與測量時產生的誤差，以及拍攝的時的視距，這樣的誤差在可接受範圍之內。

應用

將上述理論應用到實際當中，我用130w的工業相機在距離800上對目標拍攝了一系列的圖。

誤差分析

該測量的誤差來源於以下幾個方面：

1. 安裝、測量誤差

   這個誤差是由於在裝置安裝，以及尺寸測量中所形成的。最典型的比如說在用馬克筆畫點時畫歪了一點，又或在用尺子測量P5點高度時度數不準等。

2. 像點座標的選取誤差

   這一誤差是在確定幾個點的畫素座標時，取點不準所造成的。不過由於本文用的影象有2000w畫素，因此該誤差不太明顯。若是用100w的影象，該誤差的影響就會被大大增強。

3. 兩張拍照位置造成的誤差。

   理論上兩次拍照位置相互垂直時，最後計算出來的P點世界座標的誤差最小，如下圖。

   

但實際情況卻不一定這麼理想，尤其是在處理無人機拍攝的視訊時，可能隔幾幀就進行一次處理，這樣就會帶來較大的誤差，所以最好的辦法是求多組值，取它們的加權平均值。像本文所用的兩幅影象的拍攝角度大概是這樣的：

主檢視：

側檢視：

用這兩幅影象處理產生的誤差就會比較大，但最終計算出的誤差也就在1mm左右，能夠接受。用這兩幅圖做實驗是因為作者比較懶惰，直接用了以前拍的圖片，而沒有重新採集影象，好同志不要學。