RoboMaster視覺教程（5）目標位置解算（通過像素點獲取轉角）

概覽

在識別到目標後，有一個很重要的問題：我們的最終目的是瞄準、跟蹤、打擊，怎樣利用識別到目標後得到的目標在圖像中的像素座標來確定在真實世界中目標的位置呢？更清楚點說就是我識別得到的是圖像中點的座標，而我要輸出告訴下位機的是它應該旋轉或者移動到的目的地。

直接使用像素座標的缺陷

在RoboMaster視覺輔助瞄準中我們需要讓槍管一直瞄準裝甲板，由於攝像頭是裝在槍管上的，一種顯而易見的方法就是直接利用目標所在像素的座標與圖像中心座標對比，然後利用一套控制算法移動攝像頭使目標在攝像頭的中心區域（東北林業大學ARES戰隊的開源代碼是這種方式 GitHub：https://github.com/moxiaochong/NEFU-ARES-Vison-Robomaster2018）。

直接這樣做的話PID調節會很麻煩，可以看到東林的代碼中PID部分根據像素差的不同使用了不同的PID值。

像素差如果直接餵給PID，那PID只能知道我有偏差了需要照着這個方向轉減小偏差，如果偏差大了就給大點偏差小了就少給點。

而發給戰車雲臺的數據是旋轉的角度，所以像素差與角度之間的關係需要不斷調節PID來控制。並且因爲像素差與角度並不是線性關係，導致在圖像的不同地方需要不同的PID值。如下圖：

可以看到如果角度在30度以下的時候角度與像素差的關係還可以近似線性，當大於30度後就無法近似了。

那怎樣將像素差轉化成角度呢？根據上面的圖可以看出如果能夠得到OF的距離就可以通過反正切函數來得到角度值了，而OF其實就是焦距，當然上圖是理論情況，在實際應用中我們需要通過相機標定來得到需要的相機內參。

攝像頭標定

因爲我們需要從圖像中得到關於物理世界的信息，所以需要標定攝像頭來得到攝像頭內參。

攝像頭標定的工具有很多，OpenCV自帶例子中就有標定工具，也可以用MATLAB來進行標定，標定方法可以參考：https://blog.csdn.net/Loser__Wang/article/details/51811347

根據小孔成像原理得到需要的轉角

在標定完攝像頭後我們會得到攝像頭的內參矩陣和畸變參數：
$cameraMatrix=\left[ \begin{matrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 &1 \end{matrix} \right] distortionCoefficients=(k_1 \ k_2 \ p_1 \ p_2 \ k_3)$
首先根據攝像頭內參矩陣和畸變參數矯正x和y的像素值，之後通過計算可以得到轉角，原理在《Learning OpenCV》的第11章「攝像頭模型與標定」中有。

像素點與物理世界座標的關係：其中X、Y、Z爲物理世界中點的座標
$x_{screen}=f_x(\frac{X}{Z})+c_x\\ \\ y_{screen}=f_y(\frac{Y}{Z})+c_y$

我們要想得到轉角只要得到X/Z和Y/Z，然後通過反三角函數就可以得到需要的角度了。
$\tan\theta_x=\frac{X}{Z}=\frac{x_{screen}-c_x}{f_x}\\ \tan\theta_y=\frac{Y}{Z}=\frac{y_{screen}-c_y}{f_y}\\ \theta_x=\arctan(\tan\theta_x)\\ \theta_y=\arctan(\tan\theta_y)$
在東南大學的開源代碼中的位姿解算部分中的單點解算角度部分就是利用這個原理。

if(angle_solver_algorithm == 0)//One Point
{
    double x1, x2, y1, y2, r2, k1, k2, p1, p2, y_ture;
    x1 = (centerPoint.x - _cam_instant_matrix.at<double>(0, 2)) / _cam_instant_matrix.at<double>(0, 0);
    y1 = (centerPoint.y - _cam_instant_matrix.at<double>(1, 2)) / _cam_instant_matrix.at<double>(1, 1);
    r2 = x1 * x1 + y1 * y1;
    k1 = _params.DISTORTION_COEFF.at<double>(0, 0);
    k2 = _params.DISTORTION_COEFF.at<double>(1, 0);
    p1 = _params.DISTORTION_COEFF.at<double>(2, 0);
    p2 = _params.DISTORTION_COEFF.at<double>(3, 0);
    x2 = x1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p1*x1*y1 + p2 * (r2 + 2 * x1*x1);
    y2 = y1 * (1 + k1 * r2 + k2 * r2*r2) + 2 * p2*x1*y1 + p1 * (r2 + 2 * y1*y1);
    y_ture = y2 - _params.Y_DISTANCE_BETWEEN_GUN_AND_CAM / 1000;
    _xErr = atan(x2) / 2 / CV_PI * 360;
    _yErr = atan(y_ture) / 2 / CV_PI * 360;
    if(is_shooting_rune) _yErr -= _rune_compensated_angle;

    return ONLY_ANGLES;
}

不過這段代碼在我測試的時候感覺有問題，它矯正畸變的過程可能有誤。

我在實現的時候直接使用OpenCV的矯正函數對點進行矯正，得到的結果是正確的

void calAngle(Mat cam,Mat dis,int x,int y)
{
    double fx=cam.at<double>(0,0);
    double fy=cam.at<double>(1,1);
    double cx=cam.at<double>(0,2);
    double cy=cam.at<double>(1,2);
    Point2f pnt;
    vector<cv::Point2f> in;
    vector<cv::Point2f> out;
    in.push_back(Point2f(x,y));
    //對像素點去畸變
    undistortPoints(in,out,cam,dis,noArray(),cam);
    pnt=out.front();
    //沒有去畸變時的比值
    double rx=(x-cx)/fx;
    double ry=(y-cy)/fy;
    //去畸變後的比值
    double rxNew=(pnt.x-cx)/fx;
    double ryNew=(pnt.y-cy)/fy;
    //輸出原來點的座標和去畸變後點的座標
    cout<< "x: "<<x<<" xNew:"<<pnt.x<<endl;
    cout<< "y: "<<y<<" yNew:"<<pnt.y<<endl;
    //輸出未去畸變時測得的角度和去畸變後測得的角度
    cout<< "angx: "<<atan(rx)/CV_PI*180<<" angleNew:"<<atan(rxNew)/CV_PI*180<<endl;
    cout<< "angy: "<<atan(ry)/CV_PI*180<<" angleNew:"<<atan(ryNew)/CV_PI*180<<endl;
}

角度測量驗證

如何知道我們測出的角度是正確的呢？可以通過一把三角尺來進行測量。

將三角尺的30度尖對準攝像頭，不斷調整位置，使尖對準cx的像素值位置，之後不斷調整攝像頭，使三角尺的兩條邊在圖像中平行於y軸，此時就可以選取三角尺邊上的像素點來測量角度了，如下圖：

拍攝出的圖像如圖：

這個時候選擇左邊那條邊測得的角度就是30度，選擇右邊的那條邊測得的角度就是0度。

角度測量的代碼我放在GitHub上了 https://github.com/hejiangda/angleMeasure

申請了一個自己的公衆號江達小記，打算將自己的學習研究的經驗總結下來幫助他人也方便自己。感興趣的朋友可以關注一下。

RoboMaster視覺教程（5）目標位置解算（通過像素點獲取轉角）