VINS-Mono学习（六）：回环检测
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.015
if (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; // 初始化最小索引为无效值
// 遍历所有候选关键帧，寻找满足条件的最小ID
for (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); // 匹配上了状态位置1
else
status.push_back(0); // 没匹配上状态位置0
matched_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 -> Tcw
R_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全是0
for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
status.push_back(0);
// inlier部分状态位status全部置1
for( int i = 0; i < inliers.rows; i++)
{
int n = inliers.at<int>(i);
status[n] = 1;
}
// 转回eigen，以及Tcw -> Twc -> Twi
cv::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) * P2
P1 = 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归一化坐标和特征点ID
for (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]; // 特征点ID
msg_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()); // 平移X
t_q_index.values.push_back(T.y()); // 平移Y
t_q_index.values.push_back(T.z()); // 平移Z
t_q_index.values.push_back(Q.w()); // 四元数w
t_q_index.values.push_back(Q.x()); // 四元数x
t_q_index.values.push_back(Q.y()); // 四元数y
t_q_index.values.push_back(Q.z()); // 四元数z
t_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的pose
cur_kf->getVioPose(vio_P_cur, vio_R_cur); // 当前关键帧的VIO的pose
Vector3d 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();//当前最新帧的id
first_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_i
double 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;//设置最大迭代次数为5
ceres::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的list
for (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帧，并且需要他们是一个序列中的KF
for (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_i
Vector3d 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];//Δyaw
ceres::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; // 得到回环帧在这次优化中的idx
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());//四元数q_array转成旋转矩阵toRotationMatrix再转成欧拉角R2ypr
Vector3d 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);
}
}思维导图：
