整体框架参考 从零开始做自动驾驶定位(九): 建图系统结构优化
我的电脑上
rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 1.3
基本前端里程计能跟上gnss轨迹
先跑通建图
roslaunch mapping.launch
下图结果为后端刚优化完前950帧后建立出一部分全局地图的结果:
橙色 组合导航位姿 /synced_gnss(基本与绿色重合)
红色 前端里程计位姿 /transformed_odom
绿色 优化后的位姿 /optimized_key_frames
/optimized_odom(未用上)
back_end中,关键帧点云数据保存在 slam_data 文件夹下 使用的时候再去读取(节省内存)
// 把关键帧点云存储到硬盘里
std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(key_frames_deque_.size()) + ".pcd";
pcl::io::savePCDFileBinary(file_path, *cloud_data.cloud_ptr);
回环检测中订阅了两个来自后端的数据:
// subscriber
key_frame_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_frame", 100000);
key_gnss_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_gnss", 100000);
两个数据区别如下:
KeyFrame key_frame;
key_frame.time = laser_odom.time;
key_frame.index = (unsigned int)key_frames_deque_.size();
key_frame.pose = laser_odom.pose;
key_frames_deque_.push_back(key_frame);
current_key_frame_ = key_frame;
current_key_gnss_.time = gnss_odom.time;
current_key_gnss_.index = key_frame.index;
current_key_gnss_.pose = gnss_odom.pose;
/key_frame数据中存的是激光里程计的时间和位姿,/key_gnss存的是原始组合导航的
class KeyFrame:
class KeyFrame {
public:
double time = 0.0;
unsigned int index = 0;
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
public:
Eigen::Quaternionf GetQuaternion();
};
bool LoopClosingFlow::Run() {
if (!ReadData())
return false;
while(HasData()) {
if (!ValidData())
continue;
loop_closing_ptr_->Update(current_key_frame_, current_key_gnss_);
PublishData();
}
return true;
}
原始工程中通过函数DetectNearestKeyFrame
找距离最近的历史位姿做闭环匹配
提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿情况下,闭环检测的实现难度会加大。要求使用前面讲过的scan context,实现此功能。并和已有的图优化功能完成对接,实现修正。并最终给出修正前后的轨迹精度对比。
闭环检测Tips:
DetectNearestKeyFrame
中找距离最近的历史位姿 detect_area )代码中参数:
loop_step: 5 # 防止检测过于频繁,每隔loop_step个关键帧检测一次闭环
detect_area: 10.0 # 检测区域,只有两帧距离小于这个值,才做闭环匹配
diff_num: 100 # 过于小的闭环没有意义,所以只有两帧之间的关键帧个数超出这个值再做检测
前端节点中关键帧选取要求两帧实际距离大于2m:
key_frame_distance: 2.0 # 关键帧距离
这一点在函数DetectNearestKeyFrame
中设置skip_num有所体现,因为每2.0m选取一个新的关键帧,距离/2.0代表了间隔的关键帧数,用这一点重新设置skip_num(该变量用于防止检测过于频繁)
skip_num = (int)min_distance;
if (min_distance > detect_area_) {
/// front_end.yaml 中设置 key_frame_distance: 2.0 # 关键帧距离
skip_num = std::max((int)(min_distance / 2.0), loop_step_);
return false;
} else {
skip_num = loop_step_;
return true;
}
SC-LeGO-LOAM中给出了scan context的使用方法
主要使用 sc 中的如下两个函数:
makeAndSaveScancontextAndKeys()
detectLoopClosureID()
mapOptmization.cpp main函数中开启了用于回环检测的线程:
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
主函数 while (ros::ok())
中跑MO.run();
,其中saveKeyFramesAndFactor();
调用
scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
生成Scan context
在loopthread中performLoopClosure();
--> detectLoopClosure()
-->
匹配Scan context寻找可能的回环:
SCclosestHistoryFrameID = -1; // init with -1
auto detectResult = scManager.detectLoopClosureID(); // first: nn index, second: yaw diff
SCclosestHistoryFrameID = detectResult.first;
yawDiffRad = detectResult.second; // not use for v1 (because pcl icp withi initial somthing wrong...)
拼接回环附近关键帧:
// save history near key frames: map ptcloud (icp to query ptcloud)
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
if (SCclosestHistoryFrameID + j < 0 || SCclosestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
*SCnearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[SCclosestHistoryFrameID+j], &cloudKeyPoses6D->points[SCclosestHistoryFrameID+j]);
*SCnearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[SCclosestHistoryFrameID+j], &cloudKeyPoses6D->points[SCclosestHistoryFrameID+j]);
}
downSizeFilterHistoryKeyFrames.setInputCloud(SCnearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*SCnearHistorySurfKeyFrameCloudDS);
最后通过ICP匹配两组点云通过得分判断是否是真实的回环
bool LoopClosing::DetectLoopByScanContext(int& key_frame_index)
{
CloudData::CLOUD_PTR scan_cloud_ptr(new CloudData::CLOUD());
std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.back().index) + ".pcd";
pcl::io::loadPCDFile(file_path, *scan_cloud_ptr);
scan_filter_ptr_->Filter(scan_cloud_ptr, scan_cloud_ptr);
// pcl::PointCloud::Ptr CloudData::CLOUD_PTR
// pcl::PointCloud pcl::PointCloud
sc_manager_.makeAndSaveScancontextAndKeys(*scan_cloud_ptr);
static int skip_cnt = 0;
static int skip_num = loop_step_;
if (++skip_cnt < skip_num)
return false;
auto detectResult = sc_manager_.detectLoopClosureID();
int key_num = (int)all_key_gnss_.size();
if (detectResult.first > extend_frame_num_
&& detectResult.first < key_num - (diff_num_ > extend_frame_num_?diff_num_:extend_frame_num_) ) {
key_frame_index = detectResult.first;
skip_cnt = 0;
LOG(INFO) << "Maybe Loop by DetectLoopByScanContext: " << key_frame_index
<< " key_num: " << key_num
<< std::endl << std::endl;
return true;
}
else{
return false;
}
}
问题记录:
I20201115 21:01:48.951658 17607 loop_closing.cpp:188] Maybe Loop by DetectLoopByScanContext: 231 key_num: 412
I20201115 21:01:49.929672 17607 loop_closing.cpp:188] Maybe Loop by DetectLoopByScanContext: 0 key_num: 417
terminate called after throwing an instance of 'std::out_of_range'
what(): deque::_M_range_check: __n (which is 18446744073709551611)>= this->size() (which is 417)
这里是因为一开始没有控制好key_frame_index的判断,在JointMap
函数中通过加减extend_frame_num_获得一定范围内关键帧拼接为地图,索引 i 可能越界:
for (int i = key_frame_index - extend_frame_num_; i < key_frame_index + extend_frame_num_; ++i) {
std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.at(i).index) + ".pcd";
CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
pcl::io::loadPCDFile(file_path, *cloud_ptr);
Eigen::Matrix4f cloud_pose = pose_to_gnss * all_key_frames_.at(i).pose;
pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, cloud_pose);
*map_cloud_ptr += *cloud_ptr;
}
原DetectNearestKeyFrame
函数中有如下判断:
if (key_frame_index < extend_frame_num_)
return false;
对应自己的DetectLoopByScanContext
中做如下判断:
if (detectResult.first > extend_frame_num_
&& detectResult.first < key_num - (diff_num_ > extend_frame_num_?diff_num_:extend_frame_num_) ) {
key_frame_index = detectResult.first;
保证能获取前后extend_frame_num_个关键帧,且diff_num_控制历史帧与当前帧距离,保证是真实回环且足够大
有闭环加测的运行结果:
------ 完成第 1 次后端优化 -------
顶点数:950, 边数: 1899
迭代次数: 10/512
用时:0.0581203
优化前后误差变化:138334--->2.78209
------ 完成第 2 次后端优化 -------
顶点数:1900, 边数: 3799
迭代次数: 9/512
用时:0.131694
优化前后误差变化:311170--->5.18597
------ 完成第 1 次后端优化 -------
顶点数:950, 边数: 1906
迭代次数: 14/512
用时:0.124549
优化前后误差变化:119721--->7.18556
.
.
.
I20201116 13:48:17.856556 28431 back_end.cpp:122] 插入闭环:18,1890
I20201116 13:48:18.423070 28433 loop_closing.cpp:189] Maybe Loop by DetectLoopByScanContext: 23 key_num: 1896
I20201116 13:48:19.029443 28433 loop_closing.cpp:227] 检测到闭环 62: 帧23------>帧1895
fitness score: 0.0306793
I20201116 13:48:19.056519 28431 back_end.cpp:122] 插入闭环:23,1895
I20201116 13:48:19.577155 28431 g2o_graph_optimizer.cpp:41]
------ 完成第 2 次后端优化 -------
顶点数:1900, 边数: 3861
迭代次数: 16/512
用时:0.298337
优化前后误差变化:402596--->33.4329
可以看到加入闭环检测后第 2 次后端优化的总边数从3799增加至3861,检测到62个闭环
使用组合导航结果(gnss_pose)作为 ground truth ,对比是否加入闭环检测优化后的evo评估结果:
无闭环检测 | 有闭环检测 | |
---|---|---|
evo_ape(对齐) | max 0.515271 mean 0.037690 median 0.030179 min 0.000578 rmse 0.052507 sse 5.238282 std 0.036557 |
max 0.543990 mean 0.096268 median 0.045853 min 0.002610 rmse 0.139179 sse 36.804718 std 0.100515 |
evo_rpe | max 3.253554 mean 1.659351 median 1.643461 min 0.563084 rmse 1.766352 sse 56.159971 std 0.605435 |
max 3.120638 mean 1.721211 median 1.769140 min 0.443099 rmse 1.832433 sse 60.440625 std 0.628686 |
从结果来看加入闭环的结果反而还差一些(我也很疑惑反复确认没有弄反),个人理解是本身后端优化后的结果已经足够好,而且后端优化中是通过添加 激光里程计对应的边 和 gnss位置对应的先验边 结果自然是平差后的且和gnss结果很接近(设置了较大权重),而我们ground truth就是直接使用gnss结果,添加了回环约束后反而会把结果拉离远gnss结果,但这不代表加入闭环效果不好,因为我们其实ground truth选取就是存在一定问题的(真正的ground truth也不可能得到,这也是为什么我们需要多传感器融合定位)
提供的工程框架中已经给出了位姿初始化功能,但是是在起始位置的,并且是基于已知粗略位姿的。要求实现地图中任意位置的位姿初始化,可以从三种难度等级中任选一种,难度越高,得分越高。
按照以下思路:
运行mapping.launch
建图的同时保存初始GNSS的latitude, longitude, altitude(地图原点)
函数DataPretreatFlow::InitGNSS()
--> GNSSData::InitOriginPosition()
中获取原点数据:
void GNSSData::InitOriginPosition() {
geo_converter.Reset(latitude, longitude, altitude);
origin_longitude = longitude;
origin_latitude = latitude;
origin_altitude = altitude;
LOG(INFO) << "origin_longitude: " << origin_longitude
<< " origin_latitude: " << origin_latitude
<< " origin_altitude: " << origin_altitude
<< std::endl << std::endl;
origin_position_inited = true;
}
因为ROS通讯机制的原因每次获得结果可能略有差别:
I20201118 00:32:37.104115 13747 gnss_data.cpp:25] origin_longitude: 8.39044 origin_latitude: 48.9826 origin_altitude: 116.394
I20201118 00:51:54.792853 17949 gnss_data.cpp:25] origin_longitude: 8.39044 origin_latitude: 48.9826 origin_altitude: 116.395
I20201118 00:53:21.346664 18536 gnss_data.cpp:25] origin_longitude: 8.39045 origin_latitude: 48.9827 origin_altitude: 116.396
通过三次建图初始化获得上述三组数据,我们根据此设定原点为:
同时结束建图后存储地图:
rosservice call /save_map
存储的滤波后的地图:
通过地图匹配时设置之前记录的原点数据用于初始化:
void GNSSData::InitOriginPosition() {
longitude = 8.39044;
latitude = 48.9826;
altitude = 116.395;
geo_converter.Reset(latitude, longitude, altitude);
origin_longitude = longitude;
origin_latitude = latitude;
origin_altitude = altitude;
LOG(INFO) << "origin_longitude: " << origin_longitude
<< " origin_latitude: " << origin_latitude
<< " origin_altitude: " << origin_altitude
<< std::endl << std::endl;
origin_position_inited = true;
}
运行matching.launch
基于地图匹配定位,此时先play rosbag,再启动matching.launch
,既在当前bag播放到的位置初始化。利用建图时存储的原点的latitude, longitude, altitude以及此时接收到的第一帧GNSS数据,计算位置差获得当前粗略位置再进一步匹配
下面两张图前一张是未设置存储的gnss原点从bag包一播放就启动初始化的结果(该地图原点启动);第二张是设置建图时存储的的gnss位置作为先验原点后从任一位置初始化结果(轨迹开始的地方代表初始化成功的位置):