VINS-MONO——Pose_Graph重定位和位姿图优化
VINS-MONO系列代码解读
VINS-MONO——Pose_Graph重定位和位姿图优化
- VINS-MONO系列代码解读
- 前言——无功利做事，努力达到“心流”状态
- 一、重定位
-
- 1、数据对齐，找到同一帧pose、point、image数据
- 2、判断是否为关键帧
- 3、创建关键帧并计算描述子
-
- （1）computeWindowBRIEFPoint()计算窗口内已经存在的关键点描述子
- （2）computeBRIEFPoint()
- 4、回环检测
- 5、PNP 求回环相对位姿Tbi<-bj
- 二、4DoF位姿优化——两种类型
-
- 1：Sequential Edge
- 2：loop_edge
- 代码实现
-
- *非闭环关键帧*(直接复制过来的，空格没删，谅解)
- 闭环关键帧构造边
- 参考
前言——无功利做事，努力达到“心流”状态
重定位和4自由度位姿优化在pose_graph_node节点同时双线程并行，它和后端优化的关系可以参见崔神的注解1。上图中，蓝线为正常的闭环优化流程，即通过后端的非线性优化来更新滑窗内所有相机的位姿。紫线为闭环检测模块， 当后端优化完成后，会将滑窗内的次新帧进行闭环检测，即首先提取新角点并进行描述，然后与数据库进行检索，寻找闭环帧，并将该帧添加到数据库中。红线为快速重定位模块，当检测到闭环帧后，会将闭环约束添加到后端的整体目标函数中进行非线性优化，得到第 i 帧（注意这里的帧为闭环帧中的老帧）经过滑窗优化后的位姿，从而计算出累积的偏移误差，进而对滑窗内的位姿进行修正。绿线为发布闭环优化后消除累积误差的帧Tw2<-bj，更新滑窗的Tw2<-bj。
提示：以下是本篇文章正文内容
一、重定位
1、数据对齐，找到同一帧pose、point、image数据
时间顺序上 img.heder.stamp = point.heder.stamp >= pose.header.stamp，原因在于IMU的频率大于图像采集频率，在IMU预积分时（在estimator节点进行），采用线性插值的方法使得在如图所示的WINDOW_SIZE - 2 处，IMU位姿刚好和图像的位姿一样，但是最后积分出来的时间戳还是用的在WINDOW_SIZE - 2 内的最后一帧IMU时间戳，这就导致位姿时间戳小于等于图像时间戳。point的时间戳和image一样，因为point是从image内提取出来的，赋值也是直接用的image时间戳。
2、判断是否为关键帧
判断和上一帧位姿之差大于0即可
if((T - last_t).norm() > SKIP_DIS)
3、创建关键帧并计算描述子
（1）computeWindowBRIEFPoint()计算窗口内已经存在的关键点描述子
void KeyFrame::computeWindowBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
for(int i = 0; i < (int)point_2d_uv.size(); i++)
{
cv::KeyPoint key;
key.pt = point_2d_uv[i];
window_keypoints.push_back(key);
}
extractor(image, window_keypoints, window_brief_descriptors);
}
（2）computeBRIEFPoint()
临时生成500个Harris或者FAST关键点，需要说明的是，作者论文中说的是500个Harris关键点，但是代码中却用的是FAST关键点，而且没有指定数量。生成关键点仍然计算描述子。
//额外检测新特征点并计算所有特征点的描述子，为了回环检测
void KeyFrame::computeBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
const int fast_th = 20; // corner detector response threshold
//TODO 作者论文中提到，检测500个Harris角点作为特征，而这里却是FAST特征，并且没有规定数量，有出入，尝试注释掉看看效果
if(1)
cv::FAST(image, keypoints, fast_th, true);
else
{
vector<cv::Point2f> tmp_pts;
//检测500个新的特征点并将其放入keypoints
cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);
for(int i = 0; i < (int)tmp_pts.size(); i++)
{
cv::KeyPoint key;
key.pt = tmp_pts[i];
keypoints.push_back(key);
}
}
//计算keypoints中所有特征点的描述子
extractor(image, keypoints, brief_descriptors);
.
4、回环检测
主要的功能就是这个语句完成，利用brief描述子索引是否有评分和当前帧描述子最大的，这在ORB_SLAM2已经见识过了，也不多讲，只要注意一点，赋值给loop_index是最老帧的id。其他的处理图像拼接的函数这里都没有用上，检测到回环确实会拼接两个回环帧，但不是在这里，是在下一步findconnection()函数执行的。
//查询字典数据库，得到与每一帧的相似度评分ret
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
5、PNP 求回环相对位姿Tbi<-bj
描述子匹配，窗口中所有特征点的描述子与回环帧的所有描述子匹配，通过计算Hammings距离，阈值大于80的认为描述子匹配成功，若匹配上,返回在老帧中的3D、2D坐标。随后利用reduceVector（）剔除没有匹配上的新老帧中的2D、3D点和ID。
//描述子匹配，窗口中所有特征点的描述子与回环帧的所有描述子匹配，通过计算Hammings距离，阈值大于80的认为描述子匹配成功，若匹配上,返回在老帧中的3D 2D坐标，为下面的PNP求解做准备
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)
{
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);
else
status.push_back(0);
matched_2d_old.push_back(pt);
matched_2d_old_norm.push_back(pt_norm);
}
}
用PnPRANSAC()计算回环帧和滑窗参考坐标洗的变换矩阵Tw2<-bi，同时取出误匹配点。
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);//TODO PNP_T返回imu到世界坐标的变换Tw2<-bi
经过RANSAC剔除误匹配点仍然具有足够的匹配点，说明真的构成回环，发布出来看看（也就是第4步说的在这拼接回环帧图像）
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
cv::Mat thumbimage;
cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg();
msg->header.stamp = ros::Time(time_stamp);
pub_match_img.publish(msg);
}
随后计算回环帧之间的相对误差Tbi<-bj ，并跟新到loop_info里去
//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
//Tw2<-bi.inverse() * Tw2<-bj = Tbi<-bj
relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
relative_q = PnP_R_old.transpose() * origin_vio_R;
relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
//相对位姿检验
if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
{
has_loop = true;
loop_index = old_kf->index;
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;
}
最后，是一段相当绕的关于累计误差Tw1<-w2求解，并且利用该累计误差更新关键帧序列中的位姿，一定要注意区别：t_drift, r_drift是相对位姿（两回环之间的位姿Tbi<-bj），w_r_vio, w_t_vio是累计位姿误差Tw1<-w2，具体的说明已经注释到代码里了，看着代码会更加清晰。
if (loop_index != -1)
{
//获取回环候选帧
KeyFrame* old_kf = getKeyFrame(loop_index);
//当前帧与回环候选帧进行描述子匹配
if (cur_kf->findConnection(old_kf))//PNP求相对位姿,RANSAC剔除误匹配点,计算累积误差和相对误差
{
//earliest_loop_index为最早的回环候选帧
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); //w_P_cur、w_R_cur: Tw1<-bi
cur_kf->getVioPose(vio_P_cur, vio_R_cur); //vio_P_cur、vio_R_cur: Tw2<-bj
//获取当前帧与回环帧的相对位姿relative_q、relative_t
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT(); //得到Tbi<-bj
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
//重新计算当前帧位姿w_P_cur、w_R_cur
w_P_cur = w_R_old * relative_t + w_P_old; //Tw1<-bi * Tbi<-bj = Tw1<-bj 对应崔神图23
w_R_cur = w_R_old * relative_q;
//回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x(); //偏差
//只更新yaw轴的偏差，其他pitch、roll仍保持初始值，因为只有yaw是不可观测的，只要优化它就可以
//w_R_cur*vio_R_cur.transpose() = Tw1<-bj*Tw2<-bj.transpose() = Tw1<-w2;
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
// shift vio pose of whole sequence to the world frame
//将所有图像序列都合并到世界坐标系下
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
//Tw1<-w2
w_r_vio = shift_r;
w_t_vio = shift_t;
//Tw1-
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)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur); //Tw2<-bj
//变换成Tw1<-bj
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
//TODO 上面将Tw1<-bj跟新过了呀！！ 解：一个是当前帧，一个是关键帧序列，不一样的
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
//将当前帧放入优化队列中
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
}
}
至此，重定位流程就结束了，相较于ORB_SLAM2用到个中匹配、旋转不变性等方法，VINS-MONO中的回环检测已经十分简单了，原因在于VINS-MONO有两个角度pitch and roll是可观的，并不用构建复旋转不变直方图，而且ORB_SLAM2采用的是共视图法，每次比较特征点都要考虑到共视帧中的特征点，这样一延伸看起来代码量就大了，而VINS采用滑动窗口法，只要窗口内的地图和当前帧比较接近都拿来比较，容易选取，代码量就小了。第三点VINS构建BRIEF描述子是通过第三方库实现回环关键帧打分，ORB还自己算了BRIEF描述子。
二、4DoF位姿优化——两种类型
四自由度位姿图优化不考虑landmarks、bias等其他变量，只考虑相机的位姿（更准确的说是imu的位姿），并且由于pitch和roll为可观量，只需优化yaw和平移四个自由度即可，其主要有两种策略：
- 非闭环关键帧和其他关键帧形成边
- 另一是闭环关键帧和最早的闭环关键帧形成边（多次经过同一地点，会检测到多个和当前帧形成闭环的关键帧）。
这里需要区分当中用到的local_index，local_index表示将大于最早闭环（first_looped_index）提出来并依次用i对其复制，与此同时将大于最早闭环帧的所有帧位姿取出。为什么一定要大于最早闭环帧呢？因为在最早闭环帧之前的帧都看做不动，不做优化。所以要注意local_index不是从keyframelist.begin()开始赋值。]
1：Sequential Edge
所有关键帧序列的每一个关键帧和其前4个关键帧形成一条边，也就对应着以j为变量的4次循环啦！这里还有个判断语句if，第一个条件告诉我们不要选择local_index等于0的关键帧（也就是最早关键帧），第二个条件表示是要在同一个序列的关键帧。
2：loop_edge
这里只要理解
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
到底得到的是哪一帧的local_index就豁然开朗了，loop_index在addKeyFrame函数存入的就是最老帧的id，那么local_index就是对应着最老帧（图中的绿色块0），所以这里就是闭环帧之间形成的边！
接下来都是一些位姿的更新，需要说明一下，由于执行过4DoF位姿优化，关键帧序列的位姿都发生了变化，累积误差r_drift, t_drift是当前帧和最老帧之间产生的，就需要更新，并且用该累积误差对最老帧到最新帧之间所有帧位姿做误差修正（相当于做个缝合工作）。
另外，要注意这边有两个函数容易混淆，getPose() 和 getVioPose()。在执行完4Dof位姿优化会更新各个帧的位姿，getPose()就是得到那个更新后的位姿，而getVioPose()是没有更新过的位姿，他们两个取的变量也是不同的（分别是T_w_i, vio_T_w_i）。
最后，执行完一次4DOF优化，还要暂停2秒，给重定位留时间，而且闭环很久才会检测到一次，也不用一直工作，不然就是在做while(1)死循环，不执行任何东西。
代码实现
首先看看残差4自由度残差方程，估计值 - 观测值 = 残差
optimize4DOF在PoseGraph（）初始化就中打开线程，而观测值在传入CostFunction之前就已经构造好了，也就是relative_t 和 relative_yaw。
非闭环关键帧(直接复制过来的，空格没删，谅解)
for (int j = 1; j < 5; j++)
{
/*
* i-j >= 0 ： 该帧不能是最早的闭环关键帧， 取当前帧和比当前帧小j帧的进行4自由度位姿优化
* sequence_array[i] == sequence_array[i-j] ： 数据集序列要是一样的
* */
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
//Pw<-ij 世界坐标系下的Pi - Pj
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]);
//Pbi<-ij = Rw<-bi.inverse() * pw<-ij ,转换到bi坐标系下 对应作者论文式28 带尖号的Pi<-ij测量值
relative_t = q_array[i-j].inverse() * relative_t;//
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL,
euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]); //这里调用了FourDOFError的operator()重载括号运算符
}
}
闭环关键帧构造边
//add loop edge
//当前可能有好几帧闭环关键帧，和最早的进行4自由度位姿优化
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index); //为真不终止
//loop_index为最早的闭环id, connected_inedx为0
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
double relative_yaw = (*it)->getLoopRelativeYaw(); //yaw
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],
t_array[i]);
}
loop_edge求relative_t比较简单，之前重定位更新过一次相对误差，那个就是闭环关键帧之间的relative_t，接下来让我们进入FourDOFWeightError内的重载括号运算符看看这个残差是如何求出来的（雅可比矩阵没有求解析解，直接用ceres自动求导的方法)：
template <typename T>
bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
//式28的估计值 - 测量值 = residuals
// euler to rotation
T w_R_i[9]; //Rw<-bi
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
// rotation transpose
T i_R_w[9];//Rw<-bi.inverse()
RotationMatrixTranspose(w_R_i, i_R_w);
// rotation matrix rotate point
T t_i_ij[3]; //Rw<-bi.invesr() * (Pj-Pi);
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x));
residuals[1] = (t_i_ij[1] - T(t_y));
residuals[2] = (t_i_ij[2] - T(t_z));
residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));
return true;
}
参考
https://github.com/StevenCui/VIO-Doc ↩︎