继上一篇博文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]。
这里只是绕了一下，理解清楚就很简单了。