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

VINS-Mono代码阅读笔记（十四）：posegraph的存储和加载

article 2025/9/12 0:35:45

本篇笔记紧接着VINS-Mono代码阅读笔记（十三）：posegraph中四自由度位姿优化，来分析位姿图的存储和加载。

完整（也是理想的）的SLAM的使用应该是这样的：搭载有SLAM程序的移动设备在一个陌生环境中运行一圈来对该环境进行建图；建图完成后存储该地图；当设备再次运行在该环境中的时候加载已经创建的地图帮助定位。

VINS-Mono系统当中存储的是posegraph，先来看看论文中对posegraph的保存、加载和合并的描述，然后再看代码实现。

1.位姿图的存储、加载和合并

1）位姿图的存储结构

VINS-Mono系统当中位姿图的结构比较简单，只需要存储顶点和边，还有每个关键帧（也就是顶点）的描述子。原始图像被丢弃以减少内存消耗。具体来说，保存的第 $i$ 个关键帧的状态描述如下：

$\left [i,\hat{p}_i^w,\hat{q}_i^w,v,\hat{p}_{iv}^i,\hat{\psi }_{iv},D(u,v,des) \right ]$

对其中的元素描述如下：

$i$ -----是帧的index值；

$\hat{p}_i^{w}$ 和 $\hat{q}_i^w$ -----分别是从VIO中估计得到的帧的位置和旋转；

$v$ ------如果当前帧有闭环帧的话， $v$ 就是闭环帧的index值；

$\hat{p}_{iv}^i$ 和 $\hat{\psi }_{iv}$ ------是当前帧和其闭环帧之间的相对位置和偏航角，这两个值从重定位中获得；

$D(u,v,des)$ ------是特征集合，每个特征包含2d定位（ $u,v$ 为其坐标）和描述子 $des$ 。

2）位姿图的加载

加载关键帧的时候使用和存储的结构相同的方式来加载，每个关键帧就是位姿图当中的一个顶点。顶点的初始化位姿是 $\hat{p}_i^w$ 和 $\hat{q}_i^w$ ，闭环边通过闭环信息 $\hat{p}_{iv}^i,\hat{\psi }_{iv}$ 来建立。每一个关键帧和其相邻的多个关键帧建立了多个序列边。加载完位姿图后，立刻执行一个4自由度的位姿图。位姿图的保存和加载速度和位姿图的大小线性相关。

3）位姿图的合并

位姿图不光可以优化当前的地图，还可以将当前的地图和之前已经建立的地图进行合并。如果已经加载了一个之前创建的地图，并且发现两个地图之间有闭环连接，则可以将两个地图合并到一起。因为所有的边都是相对约束，位姿图优化可以通过闭环连接自动的将两个地图合并到一起。如论文中展示的下图所示，当前地图已经通过闭环边合入了之前的地图。每个顶点和边都是相对变量，因此，只需要固定位姿图当中的第一个顶点。

2.代码实现

1）posegraph的保存

posegraph的保存路径在配置文件中进行设置：

pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path

保存后的结果如下图所示：

最后边还有一个pose_graph.txt文件，内容如下所示：

前边说过，在command线程当中持续检测键盘输入，如果输入为's'的话，就保存位姿图。

void command()
{if (!LOOP_CLOSURE)return;while(1){//检查用户键盘输入是否为schar c = getchar();if (c == 's'){m_process.lock();//用户键盘输入s后，保存当前的位姿图(地图)posegraph.savePoseGraph();m_process.unlock();printf("save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time\n");// printf("program shutting down...\n");// ros::shutdown();}//检查用户键盘输入是否为n，为n则开始一个新的图像序列if (c == 'n')new_sequence();std::chrono::milliseconds dura(5);std::this_thread::sleep_for(dura);}
}

posegraph.savePoseGraph()用于保存位姿图，代码如下：

/*** 用户键盘输入s的时候保存当前地图(位姿图)
*/
void PoseGraph::savePoseGraph()
{m_keyframelist.lock();TicToc tmp_t;FILE *pFile;printf("pose graph path: %s\n",POSE_GRAPH_SAVE_PATH.c_str());printf("pose graph saving... \n");string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt";pFile = fopen (file_path.c_str(),"w");//fprintf(pFile, "index time_stamp Tx Ty Tz Qw Qx Qy Qz loop_index loop_info\n");list<KeyFrame*>::iterator it;//遍历关键帧列表for (it = keyframelist.begin(); it != keyframelist.end(); it++){std::string image_path, descriptor_path, brief_path, keypoints_path;//1.保存图像if (DEBUG_IMAGE){image_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_image.png";imwrite(image_path.c_str(), (*it)->image);}Quaterniond VIO_tmp_Q{(*it)->vio_R_w_i};Quaterniond PG_tmp_Q{(*it)->R_w_i};Vector3d VIO_tmp_T = (*it)->vio_T_w_i;Vector3d PG_tmp_T = (*it)->T_w_i;//2.往pose_graph.txt文件当中写入位姿图相关信息fprintf (pFile, " %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f %f %d\n",(*it)->index, (*it)->time_stamp, VIO_tmp_T.x(), VIO_tmp_T.y(), VIO_tmp_T.z(), PG_tmp_T.x(), PG_tmp_T.y(), PG_tmp_T.z(), VIO_tmp_Q.w(), VIO_tmp_Q.x(), VIO_tmp_Q.y(), VIO_tmp_Q.z(), PG_tmp_Q.w(), PG_tmp_Q.x(), PG_tmp_Q.y(), PG_tmp_Q.z(), (*it)->loop_index, (*it)->loop_info(0), (*it)->loop_info(1), (*it)->loop_info(2), (*it)->loop_info(3),(*it)->loop_info(4), (*it)->loop_info(5), (*it)->loop_info(6), (*it)->loop_info(7),(int)(*it)->keypoints.size());// write keypoints, brief_descriptors   vector<cv::KeyPoint> keypoints vector<BRIEF::bitset> brief_descriptors;assert((*it)->keypoints.size() == (*it)->brief_descriptors.size());//3.保存描述子brief_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_briefdes.dat";std::ofstream brief_file(brief_path, std::ios::binary);//4.保存关键点keypoints_path = POSE_GRAPH_SAVE_PATH + to_string((*it)->index) + "_keypoints.txt";FILE *keypoints_file;keypoints_file = fopen(keypoints_path.c_str(), "w");for (int i = 0; i < (int)(*it)->keypoints.size(); i++){brief_file << (*it)->brief_descriptors[i] << endl;fprintf(keypoints_file, "%f %f %f %f\n", (*it)->keypoints[i].pt.x, (*it)->keypoints[i].pt.y, (*it)->keypoints_norm[i].pt.x, (*it)->keypoints_norm[i].pt.y);}brief_file.close();fclose(keypoints_file);}fclose(pFile);printf("save pose graph time: %f s\n", tmp_t.toc() / 1000);m_keyframelist.unlock();
}

2）posegraph的加载

在posegraph节点的main函数中，有下面这一段代码：

//4.加载先前保存的位姿图
if (LOAD_PREVIOUS_POSE_GRAPH)
{printf("load pose graph\n");m_process.lock();posegraph.loadPoseGraph();m_process.unlock();printf("load pose graph finish\n");load_flag = 1;
}

也就是说posegraph节点中开始运行的时候，首先要加载已有的位姿图。其中loadPoseGraph为加载位姿图的函数，代码如下：

//加载先前保存的位姿图
void PoseGraph::loadPoseGraph()
{TicToc tmp_t;FILE * pFile;string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt";printf("lode pose graph from: %s \n", file_path.c_str());printf("pose graph loading...\n");//打开位姿图文件pFile = fopen (file_path.c_str(),"r");if (pFile == NULL){printf("lode previous pose graph error: wrong previous pose graph path or no previous pose graph \n the system will start with new pose graph \n");return;}int index;double time_stamp;double VIO_Tx, VIO_Ty, VIO_Tz;double PG_Tx, PG_Ty, PG_Tz;double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz;double PG_Qw, PG_Qx, PG_Qy, PG_Qz;double loop_info_0, loop_info_1, loop_info_2, loop_info_3;double loop_info_4, loop_info_5, loop_info_6, loop_info_7;int loop_index;int keypoints_num;Eigen::Matrix<double, 8, 1 > loop_info;int cnt = 0;//fscanf 从一个流中执行格式化输入,fscanf遇到空格和换行时结束，注意空格时也结束。while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp, &VIO_Tx, &VIO_Ty, &VIO_Tz, &PG_Tx, &PG_Ty, &PG_Tz, &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz, &PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz, &loop_index,&loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3, &loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7,&keypoints_num) != EOF) {/*printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp, VIO_Tx, VIO_Ty, VIO_Tz, PG_Tx, PG_Ty, PG_Tz, VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz, PG_Qw, PG_Qx, PG_Qy, PG_Qz, loop_index,loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7,keypoints_num);*/cv::Mat image;std::string image_path, descriptor_path;//读取当前位姿对应的imageif (DEBUG_IMAGE){image_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_image.png";image = cv::imread(image_path.c_str(), 0);}Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz);Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz);//构造位姿四元数Quaterniond VIO_Q;VIO_Q.w() = VIO_Qw;VIO_Q.x() = VIO_Qx;VIO_Q.y() = VIO_Qy;VIO_Q.z() = VIO_Qz;Quaterniond PG_Q;PG_Q.w() = PG_Qw;PG_Q.x() = PG_Qx;PG_Q.y() = PG_Qy;PG_Q.z() = PG_Qz;Matrix3d VIO_R, PG_R;VIO_R = VIO_Q.toRotationMatrix();PG_R = PG_Q.toRotationMatrix();Eigen::Matrix<double, 8, 1 > loop_info;loop_info << loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7;if (loop_index != -1)if (earliest_loop_index > loop_index || earliest_loop_index == -1){earliest_loop_index = loop_index;}// load keypoints, brief_descriptors   加载关键点和BRIEF描述子string brief_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_briefdes.dat";std::ifstream brief_file(brief_path, std::ios::binary);string keypoints_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_keypoints.txt";FILE *keypoints_file;keypoints_file = fopen(keypoints_path.c_str(), "r");vector<cv::KeyPoint> keypoints;vector<cv::KeyPoint> keypoints_norm;vector<BRIEF::bitset> brief_descriptors;//遍历关键点数for (int i = 0; i < keypoints_num; i++){BRIEF::bitset tmp_des;brief_file >> tmp_des;brief_descriptors.push_back(tmp_des);cv::KeyPoint tmp_keypoint;cv::KeyPoint tmp_keypoint_norm;double p_x, p_y, p_x_norm, p_y_norm;if(!fscanf(keypoints_file,"%lf %lf %lf %lf", &p_x, &p_y, &p_x_norm, &p_y_norm))printf(" fail to load pose graph \n");tmp_keypoint.pt.x = p_x;tmp_keypoint.pt.y = p_y;tmp_keypoint_norm.pt.x = p_x_norm;tmp_keypoint_norm.pt.y = p_y_norm;keypoints.push_back(tmp_keypoint);keypoints_norm.push_back(tmp_keypoint_norm);}brief_file.close();fclose(keypoints_file);//构建关键帧KeyFrame* keyframe = new KeyFrame(time_stamp, index, VIO_T, VIO_R, PG_T, PG_R, image, loop_index, loop_info, keypoints, keypoints_norm, brief_descriptors);//加载新构造的关键帧loadKeyFrame(keyframe, 0);//计数器，每间隔20帧发布一次topicif (cnt % 20 == 0){publish();}cnt++;}fclose (pFile);printf("load pose graph time: %f s\n", tmp_t.toc()/1000);base_sequence = 0;
}

loadKeyFrame当中将该关键帧加入了位姿图显示当中，需要注意的是这里调用loadKeyFrame的时候传入的第二个参数是0，也就是说不需要进行闭环检测。该函数中将该关键帧加入了关键帧列表，并在显示关键帧的时候显示了关键帧之间的序列边。loadKeyFrame代码如下：

void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{cur_kf->index = global_index;global_index++;int loop_index = -1;//闭环检测，这里传入的flag_detect_loop为0，也就是说在加载之前的位姿图的时候，不需要进行闭环检测if (flag_detect_loop)loop_index = detectLoop(cur_kf, cur_kf->index);else{addKeyFrameIntoVoc(cur_kf);}if (loop_index != -1){printf(" %d detect loop with %d \n", cur_kf->index, loop_index);KeyFrame* old_kf = getKeyFrame(loop_index);if (cur_kf->findConnection(old_kf)){if (earliest_loop_index > loop_index || earliest_loop_index == -1)earliest_loop_index = loop_index;m_optimize_buf.lock();//将当前关键帧加入到优化队列中进行位姿优化optimize_buf.push(cur_kf->index);m_optimize_buf.unlock();}}m_keyframelist.lock();Vector3d P;Matrix3d R;cur_kf->getPose(P, R);Quaterniond Q{R};geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;pose_stamped.pose.position.z = P.z();pose_stamped.pose.orientation.x = Q.x();pose_stamped.pose.orientation.y = Q.y();pose_stamped.pose.orientation.z = Q.z();pose_stamped.pose.orientation.w = Q.w();base_path.poses.push_back(pose_stamped);base_path.header = pose_stamped.header;//draw local connection 显示序列边if (SHOW_S_EDGE){list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();for (int i = 0; i < 1; i++){if (rit == keyframelist.rend())break;Vector3d conncected_P;Matrix3d connected_R;if((*rit)->sequence == cur_kf->sequence){(*rit)->getPose(conncected_P, connected_R);posegraph_visualization->add_edge(P, conncected_P);}rit++;}}/*if (cur_kf->has_loop){KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);Vector3d connected_P;Matrix3d connected_R;connected_KF->getPose(connected_P,  connected_R);posegraph_visualization->add_loopedge(P, connected_P, SHIFT);}*///keyframelist中加入该关键帧keyframelist.push_back(cur_kf);//publish();m_keyframelist.unlock();
}

查看全文

http://www.lryc.cn/news/2415400.html