相機位姿估計3：根據兩幅圖像的位姿估計結果求某點的世界坐標

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

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

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

@Author：VShawn

@Date:2016-11-28

@Lab: CvLab202@CSU

目錄

• 《相機位姿估計0：基本原理之如何解PNP問題》
• 《相機位姿估計1：根據四個特征點估計相機姿態》
• 《相機位姿估計1_1：OpenCV:solvePnP二次封裝與性能測試》
• 《相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態》
• 《相機位姿估計3：根據兩幅圖像的位姿估計結果求某點的世界坐標》

前言

早就寫好了....不過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點用公式映射到三維空間中，需要使用到內參數與外參數矩陣），也就是可以確定一條從相機指向點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左右，能夠接受。用這兩幅圖做實驗是因為作者比較懶惰，直接用了以前拍的圖片，而沒有重新采集圖像，好同志不要學。