Vins-Fusion初始化位姿——3D-2D：PNP求解当前帧位姿

继上一篇博文Vins-Fusion 外参camera-imu 标定，本文继续介绍Vins-Fusion初始化时，通过PNP求解当前帧位姿。

一、3D-2D：PNP

PnP是求解3D到2D点对运动的估计，描述的是知道n个3D空间的点及其投影位置，如何估计相机的位姿。因为其无需对极约束，故更少匹配点可获得较好的运动估计。特征点的3D位置可直接由三角化或者深度图确定，双目相机无需进行特定的初始化（如单目初始化时需使相机产生一定平移）。
PnP求解常用方法有，直接线性变换（DLT）和非线性优化方法。

1.优化迭代方法

2.P3P：可以使用任意4个特征点求解，不要求共面，特征点数量不为4时报错

参考论文《complete solution classification for perspective-n-point problem》

3.EPNP：只要特征点数量大于3就可求出正解

参考论文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》

源码中直接调用的cv solvePnP求解。

二、当前帧位姿3D-2D：PNP 求解流程

1.准备路标点位姿(3d坐标)及其在相机归一化平面的投影点

源码中对第一帧未作处理，因为此时路标点还未完成三角化，即无3d位置。
在关键帧中选择深度为正的关键帧，同时特征点从start_frame到当前帧frameCnt都能一直被观测到（一直被跟踪到）时，计算第一帧中特征点在世界坐标系下的位姿（先根据特征点在相机归一化平面的位置，通过外参camera-imu旋转到imu坐标系，再通过旋转矩阵Riw，得到世界坐标系下的位姿）。

vector<cv::Point2f> pts2D;
vector<cv::Point3f> pts3D;
// 遍历当前帧特征点
for (auto &it_per_id : feature)
{
	if (it_per_id.estimated_depth > 0)
	{
		int index = frameCnt - it_per_id.start_frame;
		// 特征点从start_frame图像帧到frameCnt对应的图像帧都能被观测到（一直被跟踪到了）
		if((int)it_per_id.feature_per_frame.size() >= index + 1)
		{
			// 在第一帧IMU下的坐标
			Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0];
			// 通过第一帧的位姿转到世界系 Twi
			Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];

			cv::Point3f point3d(ptsInWorld.x(), ptsInWorld.y(), ptsInWorld.z());
			cv::Point2f point2d(it_per_id.feature_per_frame[index].point.x(), it_per_id.feature_per_frame[index].point.y());
			// 世界坐标
			pts3D.push_back(point3d);
			// 像素坐标
			pts2D.push_back(point2d); 
		}
	}
}

2.根据第一帧求解到的在世界坐标系的位姿为当前帧PnP求解的世界坐标系位姿初始值，求解solvePoseByPnP(RCam, PCam, pts2D, pts3D)。

Eigen::Matrix3d RCam;
Eigen::Vector3d PCam;
// trans to w_T_cam以上一帧图像在世界坐标系下的pose作为当前帧PnP求解的初值
// 前一帧位姿 Twc = Twi * Tic
RCam = Rs[frameCnt - 1] * ric[0];
PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];

// 3d-2d求解当前帧位姿，Twc
if(solvePoseByPnP(RCam, PCam, pts2D, pts3D))
{
	// trans to w_T_imu
	// 转换到IMU系下
	Rs[frameCnt] = RCam * ric[0].transpose(); 
	Ps[frameCnt] = -RCam * ric[0].transpose() * tic[0] + PCam;

	Eigen::Quaterniond Q(Rs[frameCnt]);
}

3.solvePoseByPnP

这一部分调用的是cv solvePnP进行求解的，所以很简单。结合源码说下几个需要注意，容易产生疑惑的地方。

bool FeatureManager::solvePoseByPnP(Eigen::Matrix3d &R, Eigen::Vector3d &P, 
                                      vector<cv::Point2f> &pts2D, vector<cv::Point3f> &pts3D)
{
    Eigen::Matrix3d R_initial;
    Eigen::Vector3d P_initial;

    // w_T_cam ---> cam_T_w 
    R_initial = R.inverse();
    P_initial = -(R_initial * P);

    //printf("pnp size %d \n",(int)pts2D.size() );
    // 特征点太少
    if (int(pts2D.size()) < 4)
    {
        printf("feature tracking not enough, please slowly move you device! \n");
        return false;
    }
    cv::Mat r, rvec, t, D, tmp_r;
    cv::eigen2cv(R_initial, tmp_r);
    cv::Rodrigues(tmp_r, rvec);
    cv::eigen2cv(P_initial, t);
    cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);  
    bool pnp_succ;
    // 3d-2d Pnp 求解位姿
    pnp_succ = cv::solvePnP(pts3D, pts2D, K, D, rvec, t, 1);
    //pnp_succ = solvePnPRansac(pts3D, pts2D, K, D, rvec, t, true, 100, 8.0 / focalLength, 0.99, inliers);

    if(!pnp_succ)
    {
        printf("pnp failed ! \n");
        return false;
    }
    cv::Rodrigues(rvec, r);
    //cout << "r " << endl << r << endl;
    Eigen::MatrixXd R_pnp;
    cv::cv2eigen(r, R_pnp);
    Eigen::MatrixXd T_pnp;
    cv::cv2eigen(t, T_pnp);

    // cam_T_w ---> w_T_cam
    R = R_pnp.transpose();
    P = R * (-T_pnp);

    return true;
}

解释cv::solvePnP

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)
Parameters:
objectPoints - 世界坐标系下的控制点的坐标，vector的数据类型在这里可以使用
imagePoints - 在图像坐标系下对应的控制点的坐标。vector在这里可以使用
cameraMatrix - 相机的内参矩阵
distCoeffs - 相机的畸变系数
以上两个参数通过相机标定可以得到。
rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
flags - 
1 SOLVEPNP_ITERATIVE：只能用四个共面的点来求解。使用列文伯格马夸尔特法迭代求解。(默认)
2 SOLVEPNP_P3P：可以使用任意4个特征点求解，不要求共面，特征点数量不为4时报错。
3 SOLVEPNP_EPNP：只要特征点数量大于3就可求出正解。

程序开始相当于求解solvePnP计算的位姿rvec、tvec的初值（cv::solvePnP输出的旋转矩阵为world-cam），因为使用列文伯格马夸尔特法迭代求解，故需提供初值，或者说提供准确的初值更有利于提高收敛速度。
solvePnP求解得到位姿rvec、tvec后，再将其转成cam-world，最终得到当前帧的位姿Rs[frameCnt]，Ps[frameCnt]。
这里只是绕了一下，理解清楚就很简单了。