当前位置：首页 > news >正文

VINS-Fusion源码逐行解析：除单目+imu模式外的位姿初始化函数initFramePoseByPnP()及其内部函数

news 来源：原创 2024/9/19 9:29:51

通过之前看processImage函数，我们发现只有单目+imu模式的位姿初始化函数用的是sfm那种，也就是initialStructure()函数，而除了这个模式外的别的模式，如双目+imu、仅双目、和不是初始化过程中位姿初始化函数都使用initFramePoseByPnP()

这个函数十分简单，相信不用看笔者的注释大家也都能理解，主要就是对所有深度大于0的特征点，找到它是否在当前帧被观测到，然后把对应的2D点、3D点取出来放进一个容器里，然后结合之前imu估计的初始位姿调用solvePoseByPnP()函数来优化并更新到imu坐标系下的位姿，我们来看一下源码：

//虽然已经有imu提供的初始位姿了，但还是要调用此函数优化
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{// 如果当前帧数大于0if(frameCnt > 0){vector<cv::Point2f> pts2D;// 用于存储2D特征点vector<cv::Point3f> pts3D;// 用于存储3D特征点// 遍历所有特征点for (auto &it_per_id : feature){   // 如果特征点的深度估计值大于0if (it_per_id.estimated_depth > 0){// 计算特征点在当前帧的索引int index = frameCnt - it_per_id.start_frame;// 如果特征点在当前帧存在if((int)it_per_id.feature_per_frame.size() >= index + 1){// 将特征点从相机坐标系转换到世界坐标系Vector3d ptsInCam = ric[0] * (it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth) + tic[0];Vector3d ptsInWorld = Rs[it_per_id.start_frame] * ptsInCam + Ps[it_per_id.start_frame];// 将世界坐标系下的3D点和图像平面上的2D点存入对应的容器中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); }}}Eigen::Matrix3d RCam;Eigen::Vector3d PCam;// trans to w_T_cam// 将当前帧的旋转和平移转换到相机坐标系RCam = Rs[frameCnt - 1] * ric[0];PCam = Rs[frameCnt - 1] * tic[0] + Ps[frameCnt - 1];// 如果通过PnP算法求解位姿成功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]);//cout << "frameCnt: " << frameCnt <<  " pnp Q " << Q.w() << " " << Q.vec().transpose() << endl;//cout << "frameCnt: " << frameCnt << " pnp P " << Ps[frameCnt].transpose() << endl;}}
}

而对于solvePoseByPnP()函数，这个函数主要就是将eigen形式的矩阵等变成opencv的格式，然后调用opencv提供给的底层函数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() );// 检查2D特征点的数量是否足够if (int(pts2D.size()) < 4){// 如果2D特征点数量不足，则提示用户缓慢移动设备printf("feature tracking not enough, please slowly move you device! \n");return false;}// 定义用于PnP求解的变量cv::Mat r, rvec, t, D, tmp_r;// 将Eigen的旋转矩阵转换为OpenCV的格式cv::eigen2cv(R_initial, tmp_r);// 将旋转矩阵转换为旋转向量cv::Rodrigues(tmp_r, rvec);// 将Eigen的平移向量转换为OpenCV的格式cv::eigen2cv(P_initial, t);// 定义相机内参矩阵，这里假设为单位矩阵cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);  // 调用solvePnP函数求解位姿bool pnp_succ;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);// 如果PnP求解失败，输出错误信息并返回falseif(!pnp_succ){printf("pnp failed ! \n");return false;}// 将旋转向量转换回旋转矩阵cv::Rodrigues(rvec, r);//cout << "r " << endl << r << endl;// 将OpenCV的矩阵格式转换回Eigen的格式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;
}