單目相機比特姿求解之PNP算法原理剖析與實踐

碼道人 2021-08-15 20:42:02 阅读数:752

本文一共[544]字,预计阅读时长:1分钟~
比特 求解 pnp 算法 原理

對於視覺裏程計中,相機比特姿的求解問題極為常見。對於雙目相機,由於其可以直接計算出深度信息,所以在相機比特姿求解上十分容易。但如果我們使用的是單目相機,如何從二維圖像中求解出相機相對三維物體的比特姿就需要一定的算法來完成。本文章將介紹用於計算單目相機比特姿求解的PNP算法原理以及應用。

坐標系轉換

在介紹PNP算法之前,我們有必要先了解一下四種坐標系之間的轉換關系。這四種坐標系分別是世界坐標系——相機坐標系——圖像坐標系——像素坐標系。

1.世界坐標系到相機坐標系

設某點在世界坐標系的坐標為, 在相機坐標系下的坐標為,則有

其中

其中R稱為旋轉矩陣,T稱為平移矩陣。

2.相機坐標系到圖像坐標

設某點在相機坐標系下的坐標為,其在圖像坐標系中對應的坐標為 。由相似三角形可得

可錶示為

3.圖像坐標系到像素坐標系

設一個像素的長和寬分別為 ,設像素坐標為 ,則

4.世界坐標系到像素坐標系

綜上所述,從世界坐標系到像素坐標系的變換矩陣K為

其中, 稱為相機在u軸和v軸方向上的尺度因子。

PNP算法

首先我們以下圖為例,根據物體在世界坐標系下的3D點以及這些3D點在圖像上投影的2D點

根據餘弦定理,可得

,可得

,可得

因為首先AB,BC,AC的距離都是可以根據輸入的3D點求得,而輸入的2D點可以求解三個餘弦值(如何求解,像素坐標根據相機內參矩陣和畸變參數可以求得在歸一化圖像平面上的3D坐標,此時 z=1,故餘弦值可求)。此時未知數僅x,y兩個,所以理論上兩個未知數兩個方程,是可求的。(從x,y求PA,PB,PC也可求)。

這樣我們就相當於知道了物體的世界坐標系,然後通過3D-3D的ICP算法可以解算出相機的比特姿。

詳細的計算過程可以參考《slam十四講》中的視覺裏程計一章。

實踐

OpenCV中提供了solvePnP函數可以直接求解出旋轉矩陣和平移矩陣

首先要知道物體在世界坐標系下的坐標(至少三組)

//定義世界坐標和圖像坐標
vector<Point3d> World_Coor = {Point3f(0, 0, 0), Point3f(0, 26.5, 0), Point3f(67.5, 26.5, 0), Point3f(67.5, 0, 0)};

求得物體的二維像素坐標

//傳入圖像坐標
vector<Point2d> Img_Coor;
Img_Coor.push_back(featrue[i].bl());
Img_Coor.push_back(featrue[i].tl());
Img_Coor.push_back(featrue[i].tr());
Img_Coor.push_back(featrue[i].br());

PNP解算

solvePnP(objectSmallArmor, img_points, cameraMatrix, distcoeff, rvec, tvec, false, SOLVEPNP_IPPE);
Rodrigues(rvec, R_rvec);
// 轉換格式
R_rvec.convertTo(R_rvec, CV_64FC1);
tvec.convertTo(tvec, CV_64FC1);
// 轉成Eigen下的矩陣
Eigen::Matrix3f Rotated_matrix;
Eigen::Vector3f Tran_vector;
cv2eigen(R_rvec, Rotated_matrix);
cv2eigen(tvec, Tran_vector);

解算歐拉角

Eigen::Vector3f euler_angles = Rotated_matrix.eulerAngles(0, 1, 2);
picth = euler_angles[0] * 180 / PI;
yaw = euler_angles[1] * 180 / PI;
roll = euler_angles[2] * 180 / PI;

計算距離

distance = (COEFF_K * sqrt(Tran_vector.transpose() * Tran_vector) + COEFF_B) * cosf(pitch * PI / 180.f);

總結

PNP算法對於單目相機中求解運動物體的歐拉角和測距有著很大的用處,其運用場景廣泛。甚至可以通過狀態變換矩陣之間的轉移矩陣進而進行狀態估計,被大量運用在機器人上的視覺裏程計中。

參考內容

【高翔】視覺SLAM十四講

版权声明:本文为[碼道人]所创,转载请带上原文链接,感谢。 https://gsmany.com/2021/08/20210815204156357e.html