深蓝学院 《多传感器融合定位》 第2章作业
目录
-
- 1. 闭环修正及精度评价
-
- 1.1 闭环检测相关参数理解
- 1.2 SC-LeGO-LOAM中使用scan context方法
- 1.3 自己工程中实现scan context检测回环
- 1.4 实验结果
-
- 1.4.1 未加闭环：
- 1.4.2 加入闭环：
- 1.4.3 对比
- 2. 位姿初始化
整体框架参考 从零开始做自动驾驶定位(九): 建图系统结构优化
我的电脑上 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找距离最近的历史位姿做闭环匹配
1. 闭环修正及精度评价
提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿情况下,闭环检测的实现难度会加大。要求使用前面讲过的scan context,实现此功能。并和已有的图优化功能完成对接,实现修正。并最终给出修正前后的轨迹精度对比。
1.1 闭环检测相关参数理解
闭环检测Tips:
- 通过里程计和组合导航知道自己位置，在此位置一定范围内检测闭环 （函数
DetectNearestKeyFrame中找距离最近的历史位姿 detect_area ） - 避免离得很近的两关键帧误判为闭环，闭环两关键帧选取需要一定间隔（loop_step）
- 成功检测一次闭环后之后路径可能有较长一段仍然是重复的，因为是重复路段，这段闭环检测基本都能成功，但不希望同一路段增加太多闭环约束，对其进行稀释
代码中参数：
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;
}
1.2 SC-LeGO-LOAM中使用scan context方法
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匹配两组点云通过得分判断是否是真实的回环
1.3 自己工程中实现scan context检测回环
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.4 实验结果
有闭环加测的运行结果：
1.4.1 未加闭环：
------ 完成第 1 次后端优化 -------
顶点数：950, 边数： 1899
迭代次数： 10/512
用时：0.0581203
优化前后误差变化：138334--->2.78209
------ 完成第 2 次后端优化 -------
顶点数：1900, 边数： 3799
迭代次数： 9/512
用时：0.131694
优化前后误差变化：311170--->5.18597
1.4.2 加入闭环：
------ 完成第 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
1.4.3 对比
可以看到加入闭环检测后第 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也不可能得到，这也是为什么我们需要多传感器融合定位）
2. 位姿初始化
提供的工程框架中已经给出了位姿初始化功能,但是是在起始位置的,并且是基于已知粗略位姿的。要求实现地图中任意位置的位姿初始化,可以从三种难度等级中任选一种,难度越高,得分越高。
参考从零开始做自动驾驶定位(十四): 基于地图的定位
按照以下思路：
运行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
通过三次建图初始化获得上述三组数据，我们根据此设定原点为：
- origin_longitude: 8.39044 origin_latitude: 48.9826 origin_altitude: 116.395
同时结束建图后存储地图：
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位置作为先验原点后从任一位置初始化结果（轨迹开始的地方代表初始化成功的位置）：