OpenCV教程，OpenCV:solvePnP參數介紹

轉自https://www.jianshu.com/p/b97406d8833c

背景介紹

由于實驗室項目的原因，最近學習了基于PNP方法的絕對位姿測量。
如果場景的三維結構已知，利用多個控制點在三維場景中的坐標及其在圖像中的透視投影坐標即可求解出攝像機坐標系與表示三維場景結構的世界坐標系之間的絕對位姿關系，包括絕對平移向量t以及旋轉矩陣R，該類求解方法統稱為N點透視位姿求解（Perspective-N-Point，PNP問題）。這里的控制點是指準確知道三維空間坐標位置，同時也知道對應圖像平面坐標的點。對于透視投影來說，要使得PNP問題有確定解，需要至少三組控制點。

image.png

image.png

經典的P3P問題可以轉化為一個四面體形狀的確定問題，如圖所示。即已知條件為知道控制點 A,B,C的位置以及在攝像機中的投影坐標求棱長邊a,b,c的問題。通過余弦定理，再利用點云配準方法可以得到攝像機坐標系相對于世界坐標系的平移以及旋轉。圖中的P點相當于相機的光心，A,B,C相當于世界坐標系下已知相對位置關系的三個控制點，A',B',C'為圖像坐標系中對應的三個點。PNP解決的是純數學問題，數學證明在此處省略。

Opencv中PNP的求解函數

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)

Parameters:

objectPoints - 世界坐標系下的控制點的坐標，vector<Point3f>的數據類型在這里可以使用
imagePoints - 在圖像坐標系下對應的控制點的坐標。vector<Point2f>在這里可以使用
cameraMatrix - 相機的內參矩陣
distCoeffs - 相機的畸變系數
以上兩個參數通過相機標定可以得到。相機的內參數的標定參見：http://www.cnblogs.com/star91/p/6012425.html
rvec - 輸出的旋轉向量。使坐標點從世界坐標系旋轉到相機坐標系
tvec - 輸出的平移向量。使坐標點從世界坐標系平移到相機坐標系
flags - 默認使用CV_ITERATIV迭代法

算法使用

由于opencv2以上版本已經提供了pnp算法的api，所以使用pnp的難點變成了如何構造場景使得能使用PNP算法。目前我們使用的最簡單的方法就是使用四個點，使用物體方法使得場景中只出現這四個控制點。
代碼如下：

     #將控制點在世界坐標系的坐標壓入容器vector<Point3f> objP;Mat objM;objP.clear();objP.push_back(Point3f(0, 0, 0));objP.push_back(Point3f(150, 0, 0));objP.push_back(Point3f(150, 150, 0));objP.push_back(Point3f(0, 150, 0));Mat(objP).convertTo(objM, CV_32F);

GaussianBlur(src, src, Size(5, 5), 1.5);
imshow("濾波后的圖", src);
//二值化
cvtColor(src, src, CV_BGR2GRAY);
threshold(src, src, 180, 255, THRESH_BINARY);
imshow("二值化后的圖", src);//邊緣檢測
Canny(src, out, thresh, thresh * 3, 3);
imshow("邊緣檢測后的圖", out);//查找輪廓
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(out, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
if (contours.size() == 0)     //沒找到任何輪廓
{cout << "未找到任何輪廓！" << endl;continue;
}
else if (contours.size() != 4)   //找到的輪廓不是4個，說明之前圖像未處理好，有干擾
{cout << "找到的輪廓不是4個！" << endl;continue;
}//計算輪廓矩       
vector<Moments> mu(contours.size());
for (int i = 0; i < contours.size(); i++)
{mu[i] = moments(contours[i], false);
}//計算輪廓的質心     
vector<Point2f> mc(contours.size());
for (int i = 0; i < contours.size(); i++)
{mc[i] = Point2d(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);//points[0].push_back(mc[i]);dis[i] = sqrt(double(mc[i].x  * mc[i].x + mc[i].y * mc[i].y));dis_x[i] = mc[i].x;dis_y[i] = mc[i].y;//cout << "第" << i << "個輪廓中心為" << mc[i].x << "\t" << mc[i].y << endl;}//對四個小燈的位置排序
double min = dis[0];
double max = dis[0];
double max_x = 0;
double max_y = 0;
int min_id = 0;
int max_id = 0;
int max_x_id = 0;
int max_y_id = 0;for (int m = 1; m < contours.size(); m++)
{if (dis[m] <= min){min = dis[m];min_id = m;}if (dis[m] >= max){max = dis[m];max_id = m;}
}
for (int m = 0; m < contours.size(); m++)
{if ((m != min_id) && (m != max_id)){if (dis_x[m] >= max_x){max_x = dis_x[m];max_x_id = m;}if (dis_y[m] >= max_y){max_y = dis_y[m];max_y_id = m;}}
}
//目標四個點按順時針順序壓入points中，左上角是第一個點
points.clear();
points.push_back(mc[min_id]);
points.push_back(mc[max_x_id]);
points.push_back(mc[max_id]);
points.push_back(mc[max_y_id]);

//使用pnp解算求出相機矩陣和畸變系數矩陣Rodrigues(rotM, rvec);  //將旋轉矩陣變換成旋轉向量solvePnP(objM, Mat(points), camera_matrix, distortion_coefficients, rvec, tvec);Rodrigues(rvec, rotM);  //將旋轉向量變換成旋轉矩陣Rodrigues(tvec, rotT);

根據旋轉矩陣和平移矩陣求出旋轉角度和深度信息

1. 根據旋轉矩陣求出坐標旋轉角

//根據旋轉矩陣求出坐標旋轉角theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));theta_y = atan2(-rotM.at<double>(2, 0),sqrt(rotM.at<double>(2, 1)*rotM.at<double>(2, 1) + rotM.at<double>(2, 2)*rotM.at<double>(2, 2)));theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));//將弧度轉化為角度theta_x = theta_x * (180 / PI);theta_y = theta_y * (180 / PI);theta_z = theta_z * (180 / PI);

求解坐標系旋轉角度的求解方法參考：https://stackoverflow.com/questions/15022630/how-to-calculate-the-angle-from-roational-matrix

1. 根據旋轉矩陣和平移矩陣求出深度信息

image.png

?

Pcam代表物體在相機坐標系下的坐標，Pworld代表物體在世界坐標系下的坐標，R和T代表了將點的從世界坐標系下映射到相機坐標系下，可以知道solvePnP求出的剛好是這樣的映射關系。
使Pcam = 0，則意味著物體移到了相機坐標系的原點，球出來的Pworld代表了相機在世界坐標系中的位置，P的z軸坐標就是深度信息。
0=PR+T
P = -inverse(R)*T

?