任丘网站建设使用网站模板快速建站

VINS最后一个节点 pose_graph 是和回环检测相关内容。

 pose_graph 节点订阅 图像话题 和 vins_estimator节点发布的话题。

具体流程：

1.ros初始化、设置句柄；

2.创建一些publisher；

3.从launch文件读取参数和参数文件config中的参数；

4.回环检测下的处理，如从yaml文件读取参数、读取训练好的二进制词袋、描述子pattern、如果需要加载离线地图及对应的处理；

5.订阅七个话题；

6.创建五个话题的发布；

7.主线程process；

8.接收键盘回调值的线程，“s” 保存地图。

重点在于七个订阅话题及回调函数的处理 和主线程process。

1.订阅话题及回调

1 订阅vins_estimator节点发布的imu_propagate话题，话题中的内容是初始化成功后上一时刻PVQ递推得到当前时刻的PVQ。在回调函数中对位姿进行修正，发布经过回环修正的位姿话题camera_pose_visual。

ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);

2 订阅vins_estimator节点发布odometry话题，话题中的内容是优化后滑窗中最新的PVQ。在回调函数对位姿进行修正，然后转到相机坐标系，后续可视化相关操作。

ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);

3 订阅图像话题，将图像放到 image_buf，

ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);

4 订阅vins_estimator发布的关键帧位姿，也就是滑窗中 WINDOW_SIZE - 2 帧的位姿，回调中将关键帧位姿放到 pose_buf，

ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);

5 订阅vins_estimator发布的外参话题，回调中保存R_bc和t_bc的值

ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);

6 订阅vins_estimator发布的关键帧地图点话题，该话题中的内容是 关键帧所能看到的地图点坐标（世界系）以及地图点在归一化相机坐标系下的坐标。回调函数中将关键帧地图点信息放到point_buf，

ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);

7 订阅vins_estimator发布的 优化后的回环帧位姿 话题，话题中的内容是优化后的回环帧位姿、回环帧的yaw、回环帧对应的当前帧在回环节点中的idx。回调函数中将回环帧的信息放到loop_info中，updateKeyFrameLoop()函数利用回环帧的位姿信息来修正当前帧的位姿。

ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);

2.主线程process

2.1 时间戳对齐

原图，KF位姿以及KF对应地图点 三个 buf 中的数据需要对齐；

2.2 取时间戳对齐的数据

时间戳对齐的原图，KF和地图点信息，构造关键帧KF对象，构造函数中计算已有特征点的描述子，同时额外提取fast角点并计算描述子；

2.3 回环检测 addKeyFrame()

posegraph.addKeyFrame(keyframe, 1);

先看posegraph对象的类PoseGraph的构造函数，创建了一个线程optimize4DoF，线程一直处于休眠状态，直到optimize_buf中有了数据。

1)通过词袋进行回环检测，寻找候选的回环帧  detectLoop() 

调用词袋查询函数query()，当前KF的候选回环帧结果在ret中，并且把当前KF的描述子放到关键帧数据库中；

/*调用词袋查询接口，参数：所有特征点的描述子，查询结果ret，最多返回4个备选KF，查找距离当前至少50帧的KF（太近的话累计误差会比较小）db相当于是数据库（是由关键帧所组成的），所有特征点的描述子brief_descriptors送进接口进行查询
*/
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);// 当然也会把当前帧（描述子的信息）送进数据库中，便于后续帧的查询
db.add(keyframe->brief_descriptors);

从 ret 中找到符合要求的回环帧：

// ret 按照得分大小降序排列，确保返回的候选 KF 数目至少一个且得分满足要求
// 有效的结果 >= 1，最优的得分 > 0.05，满足这个要求后我们才认为可能检测出关键帧的回环
if (ret.size() >= 1 && ret[0].Score > 0.05) 
{// 开始遍历其他候选帧for (unsigned int i = 1; i < ret.size(); i++) {// 别的关键帧得分 >= 0.015if (ret[i].Score > 0.015) {          find_loop = true;   // 就认为找到回环了int tmp_index = ret[i].Id;if (DEBUG_IMAGE && 0) //可能是调试时临时禁用的代码{auto it = image_pool.find(tmp_index);cv::Mat tmp_image = (it->second).clone();putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));cv::hconcat(loop_result, tmp_image, loop_result);}}}
}// 如果检测到回环且当前帧序号 > 50（避免早期帧误匹配）
if (find_loop && frame_index > 50)  
{int min_index = -1;  // 初始化最小索引为无效值// 遍历所有候选关键帧，寻找满足条件的最小IDfor (unsigned int i = 0; i < ret.size(); i++){// 条件：当前候选帧得分 > 0.015，且ID比已记录的min_index更小（或min_index未初始化）if (ret[i].Score > 0.015 && (min_index == -1 || ret[i].Id < min_index)){min_index = ret[i].Id;  // 更新最小索引}}// 返回找到的最小ID（形成回环的候选关键帧）return min_index;   
}
else
{// 未检测到回环或帧序号不足，返回-1表示失败return -1;  
}

2) 如果找到回环帧，进行回环校验  findConnection()

之前是通过词袋找到了候选，还需要通过描述子的判断以及几何的校验进行最终的确认

将当前帧的描述子依次和回环帧描述子进行匹配，计算汉明距离，满足条件bestDist < 80认为匹配成功，记录回环帧匹配点的像素坐标和归一化的相机坐标

/*** @brief 将当前帧的描述子依次和回环帧描述子进行匹配，得到匹配结果* * @param[out] matched_2d_old 匹配回环帧点的像素坐标集合* @param[out] matched_2d_old_norm 匹配回环帧点的归一化相机坐标集合* @param[out] status 状态位* @param[in] descriptors_old  回环帧的描述子集合* @param[in] keypoints_old    回环帧的像素坐标* @param[in] keypoints_old_norm 回环帧的归一化坐标*/
void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,//前三个出参，后三个入参std::vector<cv::Point2f> &matched_2d_old_norm,std::vector<uchar> &status,const std::vector<BRIEF::bitset> &descriptors_old,const std::vector<cv::KeyPoint> &keypoints_old,const std::vector<cv::KeyPoint> &keypoints_old_norm)
{// 遍历当前KF 光流追踪的特征点的描述子for(int i = 0; i < (int)window_brief_descriptors.size(); i++){cv::Point2f pt(0.f, 0.f);cv::Point2f pt_norm(0.f, 0.f);/*暴力匹配法，通过遍历所有的候选描述子得到最佳匹配参数：(入参)当前帧的一个描述子、回环帧的描述子集合、回环帧像素坐标集合、回环帧归一化坐标集合、(出参)最佳匹配的像素坐标、最佳匹配的归一化相机坐标*/if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))status.push_back(1);	// 匹配上了状态位置1elsestatus.push_back(0);	// 没匹配上状态位置0matched_2d_old.push_back(pt);	// 对应的像素坐标和归一化坐标存起来matched_2d_old_norm.push_back(pt_norm);}}

已知当前帧的位姿和地图点（世界系）和回环帧的像素坐标，PnP计算回环帧的位姿T_wb

/*** @brief 通过PNP对当前帧和回环是否构成回环进行校验* * @param[in] matched_2d_old_norm 回环帧2d归一化坐标* @param[in] matched_3d 当前帧3d地图点* @param[out] status * @param[out] PnP_T_old * @param[out] PnP_R_old */
void KeyFrame::PnPRANSAC(const vector<cv::Point2f> &matched_2d_old_norm,const std::vector<cv::Point3f> &matched_3d,std::vector<uchar> &status,Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old)
{cv::Mat r, rvec, t, D, tmp_r;cv::Mat K = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0);	// 设置单位阵Matrix3d R_inital;Vector3d P_inital;// imu坐标系转成相机坐标系(当前帧的位姿)Matrix3d R_w_c = origin_vio_R * qic;	Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;// Twc -> TcwR_inital = R_w_c.inverse();P_inital = -(R_inital * T_w_c);// 转成opencv格式cv::eigen2cv(R_inital, tmp_r);cv::Rodrigues(tmp_r, rvec);cv::eigen2cv(P_inital, t);cv::Mat inliers;TicToc t_pnp_ransac;if (CV_MAJOR_VERSION < 3)//对opencv版本进行检查solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 100, inliers);else{if (CV_MINOR_VERSION < 2)solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, sqrt(10.0 / 460.0), 0.99, inliers);else// 使用当前帧位姿作为起始位姿，考虑到回环帧距离起始帧并不远// 参数：当前帧的3D点，回环帧2d归一化坐标，k内参置成单位阵,D畸变系数（因为去完畸变了所以不用考虑），//       使用初始估计，RANSAC 迭代次数，重投影误差阈值（像素），置信度，输出的内点索引solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers);}// 初始化状态位status全是0for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)status.push_back(0);// inlier部分状态位status全部置1for( int i = 0; i < inliers.rows; i++){int n = inliers.at<int>(i);status[n] = 1;}// 转回eigen，以及Tcw -> Twc -> Twicv::Rodrigues(rvec, r);Matrix3d R_pnp, R_w_c_old;cv::cv2eigen(r, R_pnp);R_w_c_old = R_pnp.transpose();Vector3d T_pnp, T_w_c_old;cv::cv2eigen(t, T_pnp);T_w_c_old = R_w_c_old * (-T_pnp);// 这是是回环帧在VIO坐标系下的位姿PnP_R_old = R_w_c_old * qic.transpose();	PnP_T_old = T_w_c_old - PnP_R_old * tic;}

根据PnP的内点数进行判断，足够才认为回环可能成功。然后计算当前帧到回环帧的相对位姿，relative_q和relative_t，以及yaw角差，由loop_info保存这些信息。如果yaw角小于30度，并且平移距离小于20，才认为真正找到了回环。

// 根据PNP的内点数进行判断，足够才认为回环可能成功
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)  // MIN_LOOP_NUM通常设为25
{/*计算当前帧到回环帧的相对位姿 T12（当前帧为2，回环帧为1）P1 = T12 * P2通过世界坐标系转换：Pw = T_wb(2) * P2P1 = T_bw(1) * Pw因此：T12 = T_bw(1) * T_wb(2) = [R_bw(1)  t_bw(1)] * [R_wb(2)  t_wb(2)][0        1       ]   [0        1       ]*/relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);relative_q = PnP_R_old.transpose() * origin_vio_R;// 计算yaw角差（roll/pitch由重力观测约束，yaw不可观需特殊处理）relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());// 合理性检查：yaw角差<30度且平移量<20米if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0) {has_loop = true;                      // 确认回环有效loop_index = old_kf->index;           // 记录回环关键帧索引// 存储相对位姿信息（平移+旋转四元数+yaw差）loop_info << relative_t.x(), relative_t.y(), relative_t.z(),relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),relative_yaw;// 快速重定位模式下的数据发布if (FAST_RELOCALIZATION) {sensor_msgs::PointCloud msg_match_points;msg_match_points.header.stamp = ros::Time(time_stamp);// 填充回环帧的2D归一化坐标和特征点IDfor (int i = 0; i < (int)matched_2d_old_norm.size(); i++) {geometry_msgs::Point32 p;p.x = matched_2d_old_norm[i].x;p.y = matched_2d_old_norm[i].y;p.z = matched_id[i];  // 特征点IDmsg_match_points.points.push_back(p);}// 打包回环帧的位姿（世界坐标系下）Eigen::Vector3d T = old_kf->T_w_i;Eigen::Matrix3d R = old_kf->R_w_i;Quaterniond Q(R);sensor_msgs::ChannelFloat32 t_q_index;t_q_index.values.push_back(T.x());  // 平移Xt_q_index.values.push_back(T.y());  // 平移Yt_q_index.values.push_back(T.z());  // 平移Zt_q_index.values.push_back(Q.w());  // 四元数wt_q_index.values.push_back(Q.x());  // 四元数xt_q_index.values.push_back(Q.y());  // 四元数yt_q_index.values.push_back(Q.z());  // 四元数zt_q_index.values.push_back(index);  // 当前帧索引msg_match_points.channels.push_back(t_q_index);// !vins_estimator节点订阅publish的话题// 发布的是回环帧的位姿pub_match_points.publish(msg_match_points); }return true;  // 回环检测成功}
}
return false;  // 默认返回失败

注： 函数最后有一个话题发布，发布回环帧的位姿、特征点信息。vins_estimator节点订阅这个话题进行回环帧的处理。  （VINS-Mono学习（三）：vins_estimator  5.4内容）

3）修正当前帧位姿

通过之前计算的当前帧和回环帧的位姿变换 矫正当前帧的位姿

        // 如果确定两者回环，findConnection确认是否构成真正意义上的回环if (cur_kf->findConnection(old_kf)){// 更新最早回环帧，用来确定全局优化的范围if (earliest_loop_index > loop_index || earliest_loop_index == -1)earliest_loop_index = loop_index;Vector3d w_P_old, w_P_cur, vio_P_cur;Matrix3d w_R_old, w_R_cur, vio_R_cur;old_kf->getVioPose(w_P_old, w_R_old);     // 回环帧的VIO的posecur_kf->getVioPose(vio_P_cur, vio_R_cur); // 当前关键帧的VIO的poseVector3d relative_t;Quaterniond relative_q;// 得到当前帧和回环帧之间相对的平移. (findConnection函数中计算相对位姿)relative_t = cur_kf->getLoopRelativeT(); // 得到当前帧和回环帧之间相对的旋转relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();// !回环矫正后当前帧的位姿// !实际上此时有两个当前帧的位姿，一个通过回环检测后当前帧的位姿，一个是VIO计算的当前帧位姿w_P_cur = w_R_old * relative_t + w_P_old;w_R_cur = w_R_old * relative_q;double shift_yaw;Matrix3d shift_r;//两个地图点之间的位姿变换Vector3d shift_t; // 回环矫正前的位姿认为是T_w'_cur// 下面求得是 T_w_cur * T_cur_w' = T_w_w' 两个坐标系之间的相对位姿变换shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; // 如果这两个不是同一个sequence，并且当前sequence没有跟之前合并（类似是一个多地图合并的操作）if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)//首先判断当前和原来的是不是同一个地图，当前的这个地图是不是没有合并的{  //下面是合并相关的事情w_r_vio = shift_r;w_t_vio = shift_t;// T_w_w' * T_w'_cur //当前这个点的位姿转移到了老地图坐标系下的位姿vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;//旋转和平移的转化vio_R_cur = w_r_vio *  vio_R_cur;cur_kf->updateVioPose(vio_P_cur, vio_R_cur);    // 更新当前帧位姿list<KeyFrame*>::iterator it = keyframelist.begin();// 同时把这个序列当前帧之间的位姿都更新过来for (; it != keyframelist.end(); it++)   //除了当前帧（已经转化过了）的所有帧全部转到上一个地图的参考系下面去{if((*it)->sequence == cur_kf->sequence)//sequence和当前的相同，我们就把它取出来{Vector3d vio_P_cur;Matrix3d vio_R_cur;(*it)->getVioPose(vio_P_cur, vio_R_cur);//取出来vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;//做一个补偿vio_R_cur = w_r_vio *  vio_R_cur;(*it)->updateVioPose(vio_P_cur, vio_R_cur);//结果赋值回去}}// 代表这个序列已经跟之前的序列合并过了，这里实现的也就是一个序列的合并sequence_loop[cur_kf->sequence] = 1;}//这样所有新地图的关键帧坐标系全部统一回了老地图的坐标系下去m_optimize_buf.lock();// !optimize_buf.push(cur_kf->index);   // 相当于已经告诉你回环信息了，通知4dof优化线程开始干活m_optimize_buf.unlock();}

代码最后把当前关键帧的索引放到optimize_buf中，线程optimize4DoF开始工作。

到此主线程process内容基本结束。

3.回环帧位姿优化

线程optimize4DoF，优化四自由度的关键帧位姿。

注：ORB-SLAM 采用 单目相机，由于单目相机无法直接感知真实尺度（scale），整个系统的漂移可能发生在 7个自由度 上：3个平移自由度（X, Y, Z）、3个旋转自由度（roll, pitch, yaw）、1个尺度自由度（scale）。IMU 的加速度计提供了一个绝对的尺度参考，不会像纯视觉那样出现尺度不确定性；陀螺仪提供了准确的角速度测量，使得系统可以很好地估计和校正 roll 和 pitch，IMU 无法感知绝对的方向（除非有磁力计），所以偏航角（yaw）仍然可能漂移。因此，单目VINS 只会在 4 个自由度上漂移（X, Y, Z 位置 + yaw 角），而不会在 scale、roll 和 pitch 上漂移。

/*** @brief 如果找到回环，该线程就执行位姿优化* */
void PoseGraph::optimize4DoF()//四自由度位姿的优化是个线程
{while(true){int cur_index = -1;int first_looped_index = -1;m_optimize_buf.lock();// 取出最新的形成回环的当前帧while(!optimize_buf.empty())//检查是否为空，一直在等待buffer是否在填充{cur_index = optimize_buf.front();//当前最新帧的idfirst_looped_index = earliest_loop_index;   // 找到最早的回环帧optimize_buf.pop();}m_optimize_buf.unlock();if (cur_index != -1)//检测出回环{printf("optimize pose graph \n");TicToc tmp_t;m_keyframelist.lock();KeyFrame* cur_kf = getKeyFrame(cur_index);  // 取出当前帧对应的KF指针int max_length = cur_index + 1; // 预设最大长度，总之优化帧数不可能超过这么多// w^t_i   w^q_idouble t_array[max_length][3];Quaterniond q_array[max_length];double euler_array[max_length][3];double sequence_array[max_length];// 定义一个ceres优化问题，这里只优化位移和yaw角ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;options.max_num_iterations = 5;//设置最大迭代次数为5ceres::Solver::Summary summary;ceres::LossFunction *loss_function;loss_function = new ceres::HuberLoss(0.1);//设置了核函数// !由于yaw不满足简单的增量更新方式，因此需要自定义其local param形式ceres::LocalParameterization* angle_local_parameterization =AngleLocalParameterization::Create();  //保证yaw角在正负Π之间list<KeyFrame*>::iterator it;int i = 0;// 遍历KF的listfor (it = keyframelist.begin(); it != keyframelist.end(); it++){if ((*it)->index < first_looped_index)  // idx小于最早回环帧就算了continue;(*it)->local_index = i; // 这个是在本次优化中的idx，从0开始累计Quaterniond tmp_q;Matrix3d tmp_r;Vector3d tmp_t;(*it)->getVioPose(tmp_t, tmp_r);    //  得到VIO的位姿tmp_q = tmp_r;t_array[i][0] = tmp_t(0);//平移转成数组的形式t_array[i][1] = tmp_t(1);t_array[i][2] = tmp_t(2);q_array[i] = tmp_q;//旋转是四元数数组// 四元数转欧拉角Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());//分解成欧拉角的形式euler_array[i][0] = euler_angle.x();//yaw角euler_array[i][1] = euler_angle.y();//pitch角euler_array[i][2] = euler_angle.z();//roll角sequence_array[i] = (*it)->sequence;// 只有yaw角参与优化，成为参数块problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);//（yaw角，一个自由度，声明需要局部参数化不符合普通的加法，用自定义的加法选择更新方式）problem.AddParameterBlock(t_array[i], 3);//优化问题的每一个平移都会加到问题中来// 最早回环帧以及加载进来的地图保持不变，不进行优化if ((*it)->index == first_looped_index || (*it)->sequence == 0)//起始的第一帧索引需要固定住防止0空间漂移，sequence == 0代表的是事先加载的地图，vins不对事先加载的地图做任何处理{   problem.SetParameterBlockConstant(euler_array[i]);//yaw角固定住problem.SetParameterBlockConstant(t_array[i]);//平移向量固定住}// 建立约束，每一帧和之前5帧建立约束关系，找到这一帧前面5帧，并且需要他们是一个序列中的KFfor (int j = 1; j < 5; j++){if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])//前面不然没有意义//后面同一个地图点里面才会有比较好的约束效果{// 计算T_i-j_w * T_w_i = T_i-j_iVector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());//前几帧欧拉角的信息Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);//当前帧的平移-前几帧（i-j）的平移得到相对的平移关系relative_t = q_array[i-j].inverse() * relative_t;//在i-j帧以它为单位阵相对的平移向量double relative_yaw = euler_array[i][0] - euler_array[i-j][0];//Δyawceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());//用的是自动求导（入参：两帧之间相对的平移变化，两帧之间相对的yaw角的变化，i-j帧相对的pitch和ro'l'l角）// 对i-j帧和第i帧都成约束problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], //相邻帧不需要核函数，i-j帧的欧拉角t_array[i-j], //i-j帧的平移euler_array[i], //第i帧的欧拉角t_array[i]);//第i帧的平移}}//add loop edge// 如果这一帧有回环帧if((*it)->has_loop){assert((*it)->loop_index >= first_looped_index);//回环帧的id一定要>=最早回环帧的id，否则代码崩掉int connected_index = getKeyFrame((*it)->loop_index)->local_index;  // 得到回环帧在这次优化中的idxVector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());//四元数q_array转成旋转矩阵toRotationMatrix再转成欧拉角R2yprVector3d relative_t;relative_t = (*it)->getLoopRelativeT(); // 得到当前帧和回环帧的相对位姿double relative_yaw = (*it)->getLoopRelativeYaw();ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], //回环帧t_array[connected_index], euler_array[i], //当前帧（第i帧）t_array[i]);}if ((*it)->index == cur_index)  // 到当前帧了，不会再有添加了，结束break;i++;}m_keyframelist.unlock();ceres::Solve(options, &problem, &summary);m_keyframelist.lock();i = 0;// 将优化后的位姿恢复for (it = keyframelist.begin(); it != keyframelist.end(); it++)//遍历所有的列表{if ((*it)->index < first_looped_index)//如果小于第一个回环帧continue;Quaterniond tmp_q;tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));//欧拉角--->旋转矩阵--->四元数Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);//平移向量Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);   // 更新位姿if ((*it)->index == cur_index)//一直更新到当前帧为止，跳出去break;i++;}Vector3d cur_t, vio_t;Matrix3d cur_r, vio_r;cur_kf->getPose(cur_t, cur_r);  // 最新优化后的位姿cur_kf->getVioPose(vio_t, vio_r);   // 当前帧VIO的位姿m_drift.lock();// 计算当前帧的VIO位姿和优化后位姿差yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();//两帧间的yaw角通过相对关系计算出来r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));//vio的位姿和优化后的位姿的位姿差t_drift = cur_t - r_drift * vio_t;m_drift.unlock();it++;// 遍历当前帧之后的所有位姿，根据算得的位姿差进行调整for (; it != keyframelist.end(); it++)//一直调整到最后一帧{Vector3d P;Matrix3d R;(*it)->getVioPose(P, R);//把VIO位姿取出来P = r_drift * P + t_drift;//偏移量补偿上去R = r_drift * R;(*it)->updatePose(P, R);//更新}m_keyframelist.unlock();// 可视化部分updatePath();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}
}

思维导图：