pose_graph优化
pose_graph_node主节点功能介绍
各类订阅数据的回调函数功能介绍
imu_forward_callback
该回调函数处理内容
接收从imu预积分得到的Imu位姿，并根据Pose_graph记录的当前地图在全局惯性坐标系上的位姿，得到当前地图各帧Imu位姿，再利用Imu与相机之间的外参得到当前地图各图像帧位姿，并使用cameraposevisual将各图像帧显示出来。
该回调函数目的
快速给用户显示低延迟和高频率的位姿结果。
vio_callback
该回调函数处理内容
step1. 接收从后端优化后得到的Imu位姿，并根据Pose_graph记录的当前地图在全局惯性坐标系上的位姿，得到当前地图各帧Imu位姿，再利用Imu与相机之间的外参得到当前地图各图像帧位姿，并使用cameraposevisual将各图像帧显示出来。
step2. 插入到odometry_buf中(odometry_buf固定长度为10)，然后创建visualization_msgs::Marker类型的数据key_odometrys，其中记录odometry_buf中各图像帧的位置以及其他预设属性，最后由pub_key_odometrys发布出去。
step3. 若未开启回环功能，则创建geometry_msgs::PoseStamped，记录当前帧的位置以及其他预设属性，并插入到nav_msgs::Path类型的全局变量no_loop_path，由pub_vio_path发布出去。
该回调函数目的
快速给用户显示低延迟和高频率的位姿结果。
image_callback
该回调函数处理内容
接收前端处理的图像帧数据，并插入到image_buf中，并根据时间戳间隔判断是否是新的图像序列。
该回调函数目的
填充image_buf缓冲区，给到当前节点的主线程Process使用(这个后面会详细介绍)。
pose_callback
该回调函数处理内容
接收前端里程计的位姿数据，并插入到pose_buf中。
该回调函数目的
填充pose_buf缓冲区，给到当前节点的主线程Process使用(这个后面会详细介绍)。
extrinsic_callback
该回调函数处理内容
接收后端非线性优化估计的最新外参，并更新当前本地的外参信息tic/qic。
该回调函数目的
得到最优的外参矩阵，提供给其他回调函数使用。
point_callback
该回调函数处理内容
接收前端里程计处理的点云数据，并插入到point_buf中。
该回调函数目的
填充point_buf缓冲区，给到当前节点的主线程Process使用(这个后面会详细介绍)。
relo_relative_pose_callback
该回调函数处理内容
接收前端重定位计算得到的位姿数据，从pose_msg中获取相对位姿relative_q/relative_t以及图像帧索引index，并执行posegraph.updateKeyFrameLoop。
void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1 > &_loop_info)
{
// 获取出现重定位的关键帧
KeyFrame* kf = getKeyFrame(index);
// 更新该关键帧的回环信息(平移向量/旋转矩阵/yaw角)
kf->updateLoop(_loop_info);
// 若该回环矫正信息基本无误时
if (abs(_loop_info(7)) < 30.0 && Vector3d(_loop_info(0), _loop_info(1), _loop_info(2)).norm() < 20.0)
{
// 若开启快速重定位功能
if (FAST_RELOCALIZATION)
{
// 获取该关键帧的回环关键帧
KeyFrame* old_kf = getKeyFrame(kf->loop_index);
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
// 获取回环关键帧的位姿(Pose_graph阶段用的位姿，直接用于输出可视化给用户)
old_kf->getPose(w_P_old, w_R_old);
kf->getVioPose(vio_P_cur, vio_R_cur);
// 计算回环矫正后的该关键帧位姿和该关键帧VIO位姿之间的偏移值
Vector3d relative_t;
Quaterniond relative_q;
relative_t = kf->getLoopRelativeT();
relative_q = (kf->getLoopRelativeQ()).toRotationMatrix();
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;
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;
// 此处调整yaw_drift/r_drift/t_drift的目的是为了在调用addKeyFrame时及时更新关键帧位姿，实时显示给用户
m_drift.lock();
yaw_drift = shift_yaw;
r_drift = shift_r;
t_drift = shift_t;
m_drift.unlock();
}
}
}
该回调函数目的
接收重定位得到的相对位姿信息，给pose_graph提供当前帧到历史回环帧的信息关联，从而保证全局位姿地图的一致性。需要注意的是，快速重定位功能开启后会更新r_drift/t_drift偏移值，在后续addKeyFrame时会及时调整关键帧位姿并实时可视化给用户。
pose_graph_node主线程Process功能介绍
事实上还有一个线程command，但是和本博客的目的关系不大，更多的是用户交互上的功能(键盘控制，按s保存位姿图并关闭程序，按n开始一个新的图像序列)，因此这里终点介绍Process函数。
关键帧的选择
从image_buf/point_buf/pose_buf中取出具有相同时间戳的image_msg/point_msg/pose_msg数据，然后跳过前SKIP_FIRST_CNT帧数据(因为系统运行时间较短，没必要这么快做Pose_graph，然后每隔SKIP_CNT帧处理一次(从时序上增加关键帧的稀疏性)，紧接着将距离上一关键帧位置超过SKIP_DIS的图像帧创建成关键帧(从空间上增加关键帧的稀疏性)。
// 将位置距离超过SKIP_DIS的图像帧创建成关键帧
if((T - last_t).norm() > SKIP_DIS)
{
vector<cv::Point3f> point_3d;
vector<cv::Point2f> point_2d_uv;
vector<cv::Point2f> point_2d_normal;
vector<double> point_id;
for (unsigned int i = 0; i < point_msg->points.size(); i++)
{
cv::Point3f p_3d;
p_3d.x = point_msg->points[i].x;
p_3d.y = point_msg->points[i].y;
p_3d.z = point_msg->points[i].z;
point_3d.push_back(p_3d);
cv::Point2f p_2d_uv, p_2d_normal;
double p_id;
p_2d_normal.x = point_msg->channels[i].values[0];
p_2d_normal.y = point_msg->channels[i].values[1];
p_2d_uv.x = point_msg->channels[i].values[2];
p_2d_uv.y = point_msg->channels[i].values[3];
p_id = point_msg->channels[i].values[4];
point_2d_normal.push_back(p_2d_normal);
point_2d_uv.push_back(p_2d_uv);
point_id.push_back(p_id);
//printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
}
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
point_3d, point_2d_uv, point_2d_normal, point_id, sequence);
m_process.lock();
start_flag = 1;
posegraph.addKeyFrame(keyframe, 1);
m_process.unlock();
frame_index++;
last_t = T;
}
创建关键帧
利用时间戳，帧索引，位姿，源图像，关联的3D点/归一化2D点/去畸变特征点/特征点索引这些信息创建关键帧，调用关键帧类的构造函数。执行关键帧构造函数时，除了用输入参数更新关键帧成员数据之外，还调用了computeWindowBRIEFPoint()和computeBRIEFPoint()两个函数来创建更多的关键点(因为光靠前端提取的那些点，不太够用来这里做回环检测)。
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
point_3d, point_2d_uv, point_2d_normal, point_id, sequence);
computeWindowBRIEFPoint()
在输入参数的去畸变特征点位置上计算BRIEF描述子
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);
}
computeBRIEFPoint()
在原图上提取Fast关键点，并计算BRIEF描述子
void KeyFrame::computeBRIEFPoint()
{
BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());
const int fast_th = 20; // corner detector response threshold
if(1)
cv::FAST(image, keypoints, fast_th, true);
else
{
vector<cv::Point2f> tmp_pts;
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);
}
}
extractor(image, keypoints, brief_descriptors);
for (int i = 0; i < (int)keypoints.size(); i++)
{
Eigen::Vector3d tmp_p;
// 从图像坐标系转换到归一化相机坐标系(包括去畸变处理)
m_camera->liftProjective(Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p);
cv::KeyPoint tmp_norm;
tmp_norm.pt = cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z());
keypoints_norm.push_back(tmp_norm);
}
}
Pose_grapg添加关键帧
step1. 判断当前关键帧所在图像序列是否为新的图像序列，若是则重置当前序列的偏移信息；
step2. 获取当前图像序列的w_r_vio和w_t_vio，用来更新当前帧的位姿vio_P_cur/vio_R_cur；
step3. 执行回环检测detectLoop，若成功检测到则继续执行findconnection建立当前帧和回环帧的特征关联从而得到相对位姿relative_t/relative_q；
step4. 利用上一步的相对位姿计算回环矫正后的当前帧位姿w_P_cur/w_R_cur，并计算该位姿和step2中的位姿旋转量和平移量的偏移值(对于旋转量只计算yaw角的偏移，因为重力约束可认为roll角和pitch角不存在漂移，避免引入人为误差)，由此得到更合理的转换矩阵w_r_vio/w_t_vio；
step5. 利用上一步的位姿转换矩阵，将关键帧列表中同属于当前帧所在图像序列的关键帧都进行位姿矫正(前提是该图像序列之前未进行位姿矫正，因为若进行过位姿矫正后第二次位姿矫正可能会对该图像序列中历史关键帧的位姿造成影响)；
step6. 将当前关键帧放入到优化队列当中，在Pose_graph对象的一个单独创建的线程中进行四自由度优化(稳妥起见，当然是执行非线性优化才能获取更一致的位姿图啦!)。
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{
//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
// 判断是否创建新的图像序列，若是则重置当前序列的偏移信息
if (sequence_cnt != cur_kf->sequence)
{
sequence_cnt++;
sequence_loop.push_back(0);
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
m_drift.lock();
t_drift = Eigen::Vector3d(0, 0, 0);
r_drift = Eigen::Matrix3d::Identity();
m_drift.unlock();
}
// 获取当前关键帧的位姿vio_P_cur和vio_R_cur并更新
cur_kf->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;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;
global_index++;
// 回环检测，并更新回环候选帧索引
int loop_index = -1;
if (flag_detect_loop)
{
TicToc tmp_t;
// 执行回环检测，若检测到回环则返回回环帧全局索引loop_index
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;
// 提取回环帧与关键帧的位姿
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);
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
// 提取回环检测计算的关键帧和回环帧的相对位姿约束,计算当前帧的更新位姿w_P_cur和w_R_cur
Vector3d relative_t;
Quaterniond relative_q;
// relative_t和relative_q表示从当前帧到回环帧的相对位姿
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
// 计算回环矫正后的当前帧位姿和当前帧的VIO位姿的偏移量
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
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;
// shift vio pose of whole sequence to the world frame
// 若回环帧和当前帧不是同一组图像序列且当前帧所属图像序列从未进行过回环矫正,则将他们合并到同一世界坐标系上
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
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);
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);
m_optimize_buf.unlock();
}
}
keyframelist.push_back(cur_kf);
publish();
}
detectLoop回环检测
step1. 在BRIEF描述子数据库中查询与当前关键帧最相似的候选回环帧;
step2. 将当前关键帧的BRIEF描述子添加到BRIEF描述子数据库中;
step3. 若候选回环帧数目>1且最相似候选回环帧的相似度分数>0.05(一般是当前关键帧的邻域关键帧)，且存在其他候选回环帧的相似度分数>0.015则表示回环检测成功，并将对应的次高回环候选帧作为最终成功检测到的回环帧，若该回环帧的全局索引大于50则返回该索引；
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
// put image into image_pool; for visualization
cv::Mat compressed_image;
if (DEBUG_IMAGE)
{
int feature_num = keyframe->keypoints.size();
cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
image_pool[frame_index] = compressed_image;
}
TicToc tmp_t;
//first query; then add this frame into database!
QueryResults ret;
TicToc t_query;
// param_1:当前关键帧的Brief描述子向量
// param_2:词袋搜索结果std::vector<std::pair<int, double> >, 键表示回环帧全局索引，值表示词袋匹配分数
// param_3:设定搜索结果的回环帧最大数目
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//printf("query time: %f", t_query.toc());
//cout << "Searching for Image " << frame_index << ". " << ret << endl;
TicToc t_add;
// 将当前帧的BRIEF描述子信息添加到BRIEF数据库中
db.add(keyframe->brief_descriptors);
//printf("add feature time: %f", t_add.toc());
// ret[0] is the nearest neighbour's score. threshold change with neighour score
// ret中第一个回环帧肯定是距离当前关键帧最近的
bool find_loop = false;
cv::Mat loop_result;
if (DEBUG_IMAGE)
{
loop_result = compressed_image.clone();
if (ret.size() > 0)
putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
}
// visual loop result
if (DEBUG_IMAGE)
{
for (unsigned int i = 0; i < ret.size(); i++)
{
int tmp_index = ret[i].Id;
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
// a good match with its nerghbour
// 若候选回环帧数目>=1且回环帧最高分数>0.05
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
//if (ret[i].Score > ret[0].Score * 0.3)
// 若候选回环帧匹配分数>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);
}
}
}
/*
if (DEBUG_IMAGE)
{
cv::imshow("loop_result", loop_result);
cv::waitKey(20);
}
*/
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
min_index = ret[i].Id;
}
return min_index;
}
else
return -1;
}
findConnection寻找当前帧与回环帧的匹配关联
step1. 针对当前帧和回环关键帧进行特征匹配，建立带有3D信息的2D特征关联；
step2. 若第一步得到的关联2D特征数目超过预设阈值，则执行Ransac的Pnp，进一步剔除匹配外点的同时估计当前关键帧和回环关键帧的相对姿态PnP_T_old/PnP_R_old；
step3. 在剔除匹配外点之后若匹配数目超过预设阈值，则计算当前关键帧的VIO位姿和Pnp估计的位姿之间的旋转项偏移/平移项偏移/yaw角偏移，若后两者满足预设阈值则表示回环检测几何验证成功，并将回环关键帧的全局索引loop_index和平移项/yaw角相对偏移值封装成loop_info返回。
bool KeyFrame::findConnection(KeyFrame* old_kf)
{
TicToc tmp_t;
//printf("find Connection\n");
vector<cv::Point2f> matched_2d_cur, matched_2d_old;
vector<cv::Point2f> matched_2d_cur_norm, matched_2d_old_norm;
vector<cv::Point3f> matched_3d;
vector<double> matched_id;
vector<uchar> status;
// 当前关键帧关联的3D特征点
matched_3d = point_3d;
// 当前关键帧关联的去畸变2D特征点
matched_2d_cur = point_2d_uv;
// 当前关键帧关联的归一化特征点
matched_2d_cur_norm = point_2d_norm;
// 当前关键帧关联的特征点索引
matched_id = point_id;
TicToc t_match;
// 基于BRIEF描述子匹配搜索
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);
//printf("search by des finish\n");
status.clear();
/*
FundmantalMatrixRANSAC(matched_2d_cur_norm, matched_2d_old_norm, status);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);
*/
Eigen::Vector3d PnP_T_old;
Eigen::Matrix3d PnP_R_old;
Eigen::Vector3d relative_t;
Quaterniond relative_q;
double relative_yaw;
// 若匹配后的特征点数目超过预设阈值，则执行Pnp计算相对位姿
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
status.clear();
// 执行RANSAC的Pnp，计算当前关键帧到回环关键帧的相对姿态PnP_T_old/PnP_R_old以及关联特征点的内点status
// param_1: 回环关键帧的2d匹配特征的归一化坐标
// param_2: 当前关键帧的匹配特征的3d坐标
// param_3(out): 3d-2d匹配内点标志向量
// param_4(out): Pnp估计得到的回环关键帧到世界的平移向量
// param_5(out): Pnp估计得到的回环关键帧到世界的旋转矩阵
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);
}
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
// 在回环关键帧坐标系下从回环关键帧IMU到当前关键帧IMU的平移向量，即从当前关键帧到回环关键帧的平移向量
relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
// 从当前关键帧到回环关键帧的旋转矩阵
relative_q = PnP_R_old.transpose() * origin_vio_R;
// 相对yaw角偏移值(范围被归一化在-180至180以内)
relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
// 若relative_yaw绝对值小于30度且relative_t绝对值小于20，则表示成功检测到回环
// 因为理论上漂移不可能这么大，如果出现这么大的漂移说明解算肯定出现了问题
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;
if(FAST_RELOCALIZATION)
{
sensor_msgs::PointCloud msg_match_points;
msg_match_points.header.stamp = ros::Time(time_stamp);
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];
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());
t_q_index.values.push_back(T.y());
t_q_index.values.push_back(T.z());
t_q_index.values.push_back(Q.w());
t_q_index.values.push_back(Q.x());
t_q_index.values.push_back(Q.y());
t_q_index.values.push_back(Q.z());
t_q_index.values.push_back(index);
msg_match_points.channels.push_back(t_q_index);
pub_match_points.publish(msg_match_points);
}
return true;
}
}
return false;
}
searchByBRIEFDes基于BRIEF描述子匹配搜索
很朴素的暴力匹配方式，不展开说了…
bool KeyFrame::searchInAera(const BRIEF::bitset window_descriptor,
const std::vector<BRIEF::bitset> &descriptors_old,
const std::vector<cv::KeyPoint> &keypoints_old,
const std::vector<cv::KeyPoint> &keypoints_old_norm,
cv::Point2f &best_match,
cv::Point2f &best_match_norm)
{
cv::Point2f best_pt;
int bestDist = 128;
int bestIndex = -1;
// 遍历回环关键帧的每个BRIEF描述子
for(int i = 0; i < (int)descriptors_old.size(); i++)
{
// 计算当前关键帧的该BRIEF描述子和第i个回环关键帧BRIEF描述子之间的汉明距离值(int型)
int dis = HammingDis(window_descriptor, descriptors_old[i]);
// 更新最小距离值和对应的描述子索引
if(dis < bestDist)
{
bestDist = dis;
bestIndex = i;
}
}
//printf("best dist %d", bestDist);
// 若最小描述子距离值小于80，则记录匹配描述子对应的关键点去畸变图像位置以及归一化关键点位置并返回true
if (bestIndex != -1 && bestDist < 80)
{
best_match = keypoints_old[bestIndex].pt;
best_match_norm = keypoints_old_norm[bestIndex].pt;
return true;
}
// 否则返回false
else
return false;
}
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)
{
// 遍历关键帧window_brief_descriptors中的每个前端关键点对应的BRIED描述子
// 这里其实就是暴力匹配，可优化此处的匹配方式进一步加速
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);
}
}
基于RANSAC的Pnp估计和匹配外点剔除
核心是调用opencv的Pnp函数，这里需要注意的是理清旋转和平移向量是从哪个坐标系到哪个坐标系的，不要搞混淆！
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;
// 计算当前关键帧的相机到世界的旋转矩阵和平移向量
Matrix3d R_w_c = origin_vio_R * qic;
Vector3d T_w_c = origin_vio_T + origin_vio_R * tic;
// R_inital为世界到当前关键帧的旋转矩阵
// P_inital为世界到当前关键帧的平移向量
R_inital = R_w_c.inverse();
P_inital = -(R_inital * T_w_c);
cv::eigen2cv(R_inital, tmp_r);
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_inital, t);
// 记录3d-2d的匹配内点
cv::Mat inliers;
TicToc t_pnp_ransac;
if (CV_MAJOR_VERSION < 3)
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
solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers);
}
// status对那些标记为内点的特征点置为1，否则置为0
for (int i = 0; i < (int)matched_2d_old_norm.size(); i++)
status.push_back(0);
for( int i = 0; i < inliers.rows; i++)
{
int n = inliers.at<int>(i);
status[n] = 1;
}
cv::Rodrigues(rvec, r);
Matrix3d R_pnp, R_w_c_old;
cv::cv2eigen(r, R_pnp);
// R_pnp表示从世界到回环关键帧的旋转矩阵
// R_w_c_old表示从回环关键帧到世界的旋转矩阵
R_w_c_old = R_pnp.transpose();
Vector3d T_pnp, T_w_c_old;
// T_pnp表示从世界到回环关键帧的平移向量
cv::cv2eigen(t, T_pnp);
// T_w_c_old表示从回环关键帧到世界的平移向量
T_w_c_old = R_w_c_old * (-T_pnp);
// PnP_R_old表示从回环关键帧的IMU到世界的旋转矩阵
// PnP_T_old表示从回环关键帧的IMU到世界的平移向量
PnP_R_old = R_w_c_old * qic.transpose();
PnP_T_old = T_w_c_old - PnP_R_old * tic;
}
optimize4DoF四自由度优化
终于到了核心的四自由度优化环节了！在通常SLAM中Pose_graph优化过程中定义的优化变量是SE3，但是在VIO中由于有重力的强先验确保了roll角和pitch角的稳定计算，因此对于旋转部分只需要估计yaw角，一方面减轻了优化难度提高优化效率另一方面避免引入人为误差。
流程上其实并不复杂，需要注意的有三点:
(1). 4DOF优化的残差边的构建，这里面包括两类，一是序列边(即每个关键帧和时序上前四帧之间的相对Pose)，二是回环边(即回环检测成功产生的相对Pose)；
(2). 另外需要注意的是，最早回环检测成功的回环关键帧之前的历史关键帧对此处的Pose_graph优化没有太大的帮助(很直观的理解)；
(3). 还需要注意的是要考虑线程异步的问题，从代码中可以发现，在Pose_graph优化后只是更新关键帧列表中各个关键帧的T_w_i和R_w_i，并不直接改变关键帧的vio_T_w_i和vio_R_w_i，主要是因为若直接修正关键帧的vio_T_w_i和vio_R_w_i可能会导致VIO后端优化时出现异常，特别是当存在比较大的位姿调整时这个问题更加明显。
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
// 从优化队列当中获取最新的回环检测成功的关键帧的index
// 同时更新最早出现回环检测成功的关键帧的index
while(!optimize_buf.empty())
{
// optimize_buf中取出来的cur_index都是检测到回环的当前关键帧的全局索引
cur_index = optimize_buf.front();
// earliest_loop_index当中存放的是数据库中第一个和滑动窗口中关键帧形成闭环的关键帧的index
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);
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::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;// 设置线性增量方程的求解方式
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
// AngleLocalParameterization类的主要作用是指定yaw角优化变量的迭代更新，重载了括号运算
// 定义优化变量类型，主要是适配yaw角的迭代更新方式(这里采用的是自动求导的方式,后续可以改成手动求导)
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
// 遍历关键帧列表
// first_looped_index为第一次闭环帧的index,需要优化的关键帧为从第一次闭环帧到当前帧间的所有关键帧
// 一方面降低优化规模，另一方面维持Pose_graph精度，因为first_looped_index之前的关键帧和后续关键帧不存在回环边的约束,加上之前的关键帧对优化没有任何帮助
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
// 获取关键帧it的位姿
(*it)->getVioPose(tmp_t, tmp_r);
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();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
// 将关键帧列表中所有index>=first_looped_index的关键帧的位姿加入到参数块当中
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
// 设置约束：如果该帧是最早回环检测成功的回环帧的情况下，则固定它的位姿(肯定要约束某一帧的位姿，不然优化方向不可控)
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge 这里添加的是序列边，是指通过VIO计算的两帧之间的相对位姿，每帧分别与其前边最多四帧构成序列边
for (int j = 1; j < 5; j++)
{
// 且两帧属于同一序列
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
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]);
// 世界惯性坐标系下的相对平移量
relative_t = q_array[i-j].inverse() * relative_t;
// 世界惯性坐标系下的相对yaw角偏移量
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]);
}
}
//add loop edge
// 这里添加的是闭环边，是指检测到闭环的两帧
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
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();
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]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
{
printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
}
*/
m_keyframelist.lock();
i = 0;
// 更新关键帧列表中参与优化的各关键帧位姿
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
// 全局索引小于first_looped_index的并未参与优化，直接跳过
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();
// pose_graph优化后的位姿，用来更新关键帧中的T_w_i和R_w_i，并不直接改变关键帧的vio_T_w_i和vio_R_w_i
// 主要是考虑线程异步的问题，避免这里的位姿调整对VIO的后端优化造成影响
(*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);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
//cout << "t_drift " << t_drift.transpose() << endl;
//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
//cout << "yaw drift " << yaw_drift << endl;
it++;
// 更新关键帧列表中后续关键帧的位姿，注意这里还是未改动VIO位姿
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
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);
}
}
4DOF残差边FourDOFError与FourDOFWeightError
这两个4DOF残差边的代码实现中，关键就是重载operator()函数。如下以FourDOFWeightError为例，介绍该残差因子的代码实现。需要注意的是，相比序列边的残差因子，闭环边的残差因子在计算yaw角的残差时缩小了10倍，目的是降低闭环边在优化yaw角时的权重，因为闭环边约束总归没有序列边约束那么可靠。
struct FourDOFWeightError
{
FourDOFWeightError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
:t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){
weight = 1;
}
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];
// euler to rotation
T w_R_i[9];
// 得到时序上更早关键帧的旋转矩阵(从IMU到世界)
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
// rotation transpose
T i_R_w[9];
// 得到时序上更早关键帧的旋转矩阵(从世界到IMU)
RotationMatrixTranspose(w_R_i, i_R_w);
// rotation matrix rotate point
T t_i_ij[3];
// 得到时序更早关键帧坐标系上时序更晚关键帧的位置
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
// t_x,t_y和t_z都是测量值
// residuals前三项表示平移项残差，第四项表示旋转项yaw角的残差，注意这里yaw角的残差缩小了10倍(目的是避免优化过程中对yaw角的调整过大)
residuals[0] = (t_i_ij[0] - T(t_x)) * T(weight);
residuals[1] = (t_i_ij[1] - T(t_y)) * T(weight);
residuals[2] = (t_i_ij[2] - T(t_z)) * T(weight);
residuals[3] = NormalizeAngle((yaw_j[0] - yaw_i[0] - T(relative_yaw))) * T(weight) / T(10.0);
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double relative_yaw, const double pitch_i, const double roll_i)
{
// 4,1,3,1,3分别表示残差值维度，各个参数块的维度
return (new ceres::AutoDiffCostFunction<
FourDOFWeightError, 4, 1, 3, 1, 3>(
new FourDOFWeightError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}
double t_x, t_y, t_z;
double relative_yaw, pitch_i, roll_i;
double weight;
};
pose_graph_node节点中的单元测试
详细代码见xxx，主要包括回环检测阶段的代码，待实现。
版权声明：本文为tmfighting原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。