PnP, Perspective-n-Point

发表于 |

1. 概述

PnP是求解3D到2D点对运动的方法，它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿变换。最少只需要3个点对就可以估计相机的运动。

该方法使用的条件是，参考点为世界坐标系下的特征点，其空间位置已知，并且知道query帧中对应参考点的像素坐标。

PnP问题的求解方法有很多种：

  • 直接线性变换
  • P3P
  • 非线性优化方法——Bundle Ajustment

本节着眼于BA求解方法，其他方法暂时不做展开。

2. Bundle Ajustment

利用优化求解的思路是：最小化重投影误差——期望计算query帧的相机位姿$R, t$，它的李代数为$\xi$，空间特征点的坐标为$P_i = [X_i, Y_i, Z_i]^T$，其在query帧上的像素坐标为$u_i = [u_i, v_i]^T$，那么理论上：

构建成最小二乘问题就是：寻找最优的相机位姿$\xi$，使得误差最小化：

在使用优化库来求解之前，还有一个问题——每个误差项$e_i = u_i -\frac{1}{s_i}Kexp(\xi^{\wedge})P_i$的导数$J_i$。

回忆图优化中讲过的，优化问题最终转化成为矩阵的线性求解$H\Delta x = g$，其中矩阵$H$是由单个误差项一阶展开$e(x+\Delta x) = e(x) + J\Delta x$中的雅可比矩阵$J_i$ 构成的稀疏对称阵。

误差项是一个二维向量（像素坐标差），优化变量是一个六维向量（空间位姿李代数），因此$J$是一个2*6的矩阵。

其中$P^{‘}$是特征点转换到相机坐标系下的空间坐标：

因此误差项导数的第一项为：

误差项的第二项为变换后的点关于李代数的导数，参考李代数节：

其中$P^{‘\wedge}$是$P^{‘}$的反对称阵：

因此得到完整的雅可比矩阵：

除了优化相机位姿以外，还可以同时优化特征点的空间位置$P$：

其中的第二项为：

3. 优化库使用

构建图模型：

  • 优化变量1：节点1：query相机位姿（六维向量李代数）
  • 优化变量2：节点2：特征点空间位置（三维向量坐标描述）
  • 预测值：边n：根据当前estimate的优化量，投影到投影平面的像素坐标$z_i = h(\xi, P_i)$
  • 观测值：能够直接读出的，query帧上对应特征点的实际投影坐标$u_i$

g2o中已经提供了相近的基类（在g2o/types/sba/types_six_dof_expmap.h中）：李代数位姿节点VertexSE3Expmap、空间点位置节点VertexSBAPointXYZ、投影方程边EdgeProjectXYZ2UV。

边类里面要关注linearizeOplus函数，这个函数描述的是非线性函数进行线性化的过程中，求导的解析解（当然也可以使用数值解），最后函数给出的是每个节点的导数矩阵（$\frac{\partial e}{\partial \delta \xi} $和$\frac{\partial e}{\partial P_i}$） 。这点是Ceres库和g2o库的一点主要差别：Ceres都是使用自动的数值导数，g2o要自己求导。

1
2
3
4
5
6
7
8
9
10
11
void EdgeProjectXYZ2UV::linearizeOplus()
{
    VertexSE3Expmap * vj = static_cast<VertexSE3Expmap*>(_vertices[1]);
    VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
    ...
    ...
    _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
 
    _jacobianOplusXj(0,0) = x*y/z^2 * cam->focal_length;
    ...
}

4. OpenCV内置函数

basic：

1
2
3
4
5
void cv::solvePnP ( pts3d, pts2d, K, Mat(), r, t, false );

Mat r, t, R;
cv::solvePnP ( pts3d, pts2d, K, Mat(), r, t, false );
cv::Rodrigues ( r, R );     // 旋转向量和旋转矩阵的转换

advanced：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool cv::solvePnPRansac( 
    InputArray objectPoints,      // 3D空间坐标 vector<cv::Point3f> pts3d
    InputArray imagePoints,       // 2D像素坐标 vector<cv::Point2f> pts2d
    InputArray cameraMatrix,      // 相机内部参数矩阵 K
    InputArray distCoeffs,        // 畸变系数向量 cv::Mat()
    OutputArray rvec,             // 旋转向量
    OutputArray tvec,             // 平移向量
    bool useExtrinsicGuess = false, // If true, the function uses the provided rvec and tvec values as initial
    int iterationsCount = 100,    // Number of iterations
    float reprojectionError = 8.0,// 重投影误差最大值
    double confidence = 0.99,
    OutputArray inliers = noArray(), // Output vector that contains indices of inliers in objectPoints and imagePoints .
    int flags = SOLVEPNP_ITERATIVE   // method for solving PnP
)
  • Ransac：考虑到我们提供的匹配里面存在误匹配的情况，OpenCV采用“随机采样一致性算法”（Random Sample Consensus），从现有匹配中随机取一部分用来估计运动（PnP的解析解法最少只需要三个点就能计算相对位姿），正确的匹配结果都是近似的，从而剔除误匹配。
  • inlier：内点，函数最终给出的匹配可信的点。
  • RANSAC只采用少数几个随机点来计算PnP，容易受到噪声影响。工程上通常使用RANSAC的解作为初值，再使用非线性优化方法求解最优值。
1
2
3
4
5
6
7
8
9
10
11
12
13
// Ransac粗匹配
cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
cv::Rodrigues ( rvec, R ); 
Eigen::Matrix3d rotation_matrix = R.at<double>;
T_c_r_estimated_ = SE3d(
        SO3d(rotation_matrix),
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );
    
// BA局部优化
g2o::VertexSE3Expmap* pose new g2o::VertexSE3Expmap();
...
pose->setEstimate(g2o::SE3Quat( T_c_r_estimated_.rotationMatrix(), T_c_r_estimated_.translation()));