本文已参加「新人创造礼」活动，一起开启创造之路。

  关于视觉里程计中，相机位姿的求解问题极为常见。关于双目相机，由于其能够直接核算出深度信息，所以在相机位姿求解上十分简单。但如果咱们使用的是单目相机，怎么从二维图画中求解出相机相对三维物体的位姿就需要一定的算法来完成。本文章将介绍用于核算单目相机位姿求解的PNP算法原理以及使用。

坐标系转化

  在介绍PNP算法之前，咱们有必要先了解一下四种坐标系之间的转化联系。这四种坐标系分别是国际坐标系——相机坐标系——图画坐标系——像素坐标系。

1.国际坐标系到相机坐标系

  设某点在国际坐标系的坐标为Pw=(Xw,Yw,Zw)TPw=(Xw, Yw, Zw)^T, 在相机坐标系下的坐标为Pc=(Xc,Yc,Zc)TPc=(Xc, Yc, Zc)^T,则有

Pc=[RT01]Pw Pc=\left[\begin{matrix} R & T \\ 0&1 \end{matrix}\right]Pw

其中

R=[r11r12r13r21r22r23r31r32r33]　　　　　T=[txtytz]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称为旋转矩阵，T称为平移矩阵。

2.相机坐标系到图画坐标

  设某点在相机坐标系下的坐标为Pc=(Xc,Yc,Zc,1)TPc=(Xc, Yc, Zc, 1)^T,其在图画坐标系中对应的坐标为Pi=(Xi,Yi,1)TPi=(Xi, Yi, 1)^T 。由类似三角形可得

Xi=fXc/ZcYi=fYx/Zc\begin{aligned} Xi=fXc/Zc \\ Yi=fYx/Zc \end{aligned}

可表示为

ZcPi=[f0x000fy000010]PcZcPi=\left[\begin{matrix} f&0&x0&0\\ 0&f&y0&0\\ 0&0&1&0\\ \end{matrix}\right]Pc

3.图画坐标系到像素坐标系

  设一个像素的长和宽分别为dx,dydx,dy ,设像素坐标为Pp=(u,v,1)TPp=(u, v, 1)^T ，则

[uv1]=[1/dx0001/dy0001][XiYi1]\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]

4.国际坐标系到像素坐标系

  综上所述，从国际坐标系到像素坐标系的改换矩阵K为

K=[1/dx0001/dy0001][f0x00fy0001]=[fx0u00fyv0001]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]

  其中，fx=f/dx,fy=f/dyfx=f/dx,fy=f/dy ，fx,fyfx,fy 称为相机在u轴和v轴方向上的尺度因子。

PNP算法

  首先咱们以下图为例，根据物体在国际坐标系下的3D点以及这些3D点在图画上投影的2D点

单目相机位姿求解之PNP算法原理剖析与实践

  根据余弦定理，可得

单目相机位姿求解之PNP算法原理剖析与实践

  令y=PB/PC,x=PA/PCy=PB/PC, x=PA/PC ，可得

单目相机位姿求解之PNP算法原理剖析与实践

  令u=AB2/PC2,v=BC2/AB2,w=AC2/AB2u=AB^2/PC^2, v=BC^2/AB^2, w=AC^2/AB^2 ，可得

单目相机位姿求解之PNP算法原理剖析与实践

  由于首先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十四讲