单目相机位姿求解之PNP算法原理剖析与实践
对于视觉里程计中，相机位姿的求解问题极为常见。对于双目相机，由于其可以直接计算出深度信息，所以在相机位姿求解上十分容易。但如果我们使用的是单目相机，如何从二维图像中求解出相机相对三维物体的位姿就需要一定的算法来完成。本文章将介绍用于计算单目相机位姿求解的PNP算法原理以及应用。
坐标系转换
在介绍PNP算法之前，我们有必要先了解一下四种坐标系之间的转换关系。这四种坐标系分别是世界坐标系——相机坐标系——图像坐标系——像素坐标系。
1.世界坐标系到相机坐标系
设某点在世界坐标系的坐标为 P w = ( X w , Y w , Z w ) T Pw=(Xw, Yw, Zw)^T Pw=(Xw,Yw,Zw)T, 在相机坐标系下的坐标为 P c = ( X c , Y c , Z c ) T Pc=(Xc, Yc, Zc)^T Pc=(Xc,Yc,Zc)T,则有
P c = [ R T 0 1 ] P w Pc=\left[\begin{matrix} R & T \\ 0&1 \end{matrix}\right]Pw Pc=[R0T1]Pw
其中
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] T = [ t x t y t z ] R=\left[\begin{matrix} r11&r12&r13\\ r21&r22&r23\\ r31&r32&r33 \end{matrix}\right] T=\left[\begin{matrix} tx&ty&tz \end{matrix}\right] R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤ T=[txtytz]
其中R称为旋转矩阵，T称为平移矩阵。
2.相机坐标系到图像坐标
设某点在相机坐标系下的坐标为 P c = ( X c , Y c , Z c , 1 ) T Pc=(Xc, Yc, Zc, 1)^T Pc=(Xc,Yc,Zc,1)T,其在图像坐标系中对应的坐标为 P i = ( X i , Y i , 1 ) T Pi=(Xi, Yi, 1)^T Pi=(Xi,Yi,1)T 。由相似三角形可得
X i = f X c / Z c Y i = f Y x / Z c \begin{aligned} Xi=fXc/Zc \\ Yi=fYx/Zc \end{aligned} Xi=fXc/ZcYi=fYx/Zc
可表示为
Z c P i = [ f 0 x 0 0 0 f y 0 0 0 0 1 0 ] P c ZcPi=\left[\begin{matrix} f&0&x0&0\\ 0&f&y0&0\\ 0&0&1&0\\ \end{matrix}\right]Pc ZcPi=⎣⎡f000f0x0y01000⎦⎤Pc
3.图像坐标系到像素坐标系
设一个像素的长和宽分别为 d x , d y dx,dy dx,dy ,设像素坐标为 P p = ( u , v , 1 ) T Pp=(u, v, 1)^T Pp=(u,v,1)T ，则
[ u v 1 ] = [ 1 / d x 0 0 0 1 / d y 0 0 0 1 ] [ X i Y i 1 ] \left[\begin{matrix} u\\v\\1 \end{matrix}\right] = \left[\begin{matrix} 1/dx&0&0\\0&1/dy&0\\0&0&1\end{matrix}\right] \left[\begin{matrix} Xi\\Yi\\1\end{matrix}\right] ⎣⎡uv1⎦⎤=⎣⎡1/dx0001/dy0001⎦⎤⎣⎡XiYi1⎦⎤
4.世界坐标系到像素坐标系
综上所述，从世界坐标系到像素坐标系的变换矩阵K为
K = [ 1 / d x 0 0 0 1 / d y 0 0 0 1 ] [ f 0 x 0 0 f y 0 0 0 1 ] = [ f x 0 u 0 0 f y v 0 0 0 1 ] K=\left[\begin{matrix} 1/dx&0&0\\0&1/dy&0\\0&0&1\end{matrix}\right] \left[\begin{matrix} f&0&x0\\ 0&f&y0\\ 0&0&1\\ \end{matrix}\right]= \left[\begin{matrix} fx&0&u0\\ 0&fy&v0\\ 0&0&1\\ \end{matrix}\right] K=⎣⎡1/dx0001/dy0001⎦⎤⎣⎡f000f0x0y01⎦⎤=⎣⎡fx000fy0u0v01⎦⎤
其中， f x = f / d x , f y = f / d y fx=f/dx,fy=f/dy fx=f/dx,fy=f/dy ， f x , f y fx,fy fx,fy 称为相机在u轴和v轴方向上的尺度因子。
PNP算法
首先我们以下图为例，根据物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点
根据余弦定理，可得
令 y = P B / P C , x = P A / P C y=PB/PC, x=PA/PC y=PB/PC,x=PA/PC ，可得
令 u = A B 2 / P C 2 , v = B C 2 / A B 2 , w = A C 2 / A B 2 u=AB^2/PC^2, v=BC^2/AB^2, w=AC^2/AB^2 u=AB2/PC2,v=BC2/AB2,w=AC2/AB2 ，可得
因为首先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十四讲
喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！
