提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿情况下,闭环检测的实现难度会加大。要求使用前面讲过的scan context,实现此功能。并和已有的图优化功能完成对接,实现修正。并最终给出修正前后的轨迹精度对比。
回环代码的实现比较简单,主要是通过先验信息对关键帧进行最近邻匹配,得到回环的相似帧,再将这两组关键帧进行NDT匹配,得到相对位姿,再送到后端进行优化。
Update函数:
bool LoopClosing::Update(const KeyFrame key_frame, const KeyFrame key_gnss) {
has_new_loop_pose_ = false;
all_key_frames_.push_back(key_frame);
all_key_gnss_.push_back(key_gnss);
int key_frame_index = 0;
//使用gnss(先验信息) 寻找与当前帧最相似的回环检测
if (!DetectNearestKeyFrame(key_frame_index))
return false;
//回环匹配
if (!CloudRegistration(key_frame_index))
return false;
has_new_loop_pose_ = true;
return true;
}
DetectNearestKeyFrame()利用先验gnss信息寻找相似帧
bool LoopClosing::DetectNearestKeyFrame(int& key_frame_index) {
static int skip_cnt = 0;
static int skip_num = loop_step_;
//并不是每一次关键帧进来都做回环检测 skip_num个关键帧才做一次
if (++skip_cnt < skip_num)
return false;
//设置一个关键帧路程差
//两帧回环至少需要差diff_num_个gnss帧
if ((int)all_key_gnss_.size() < diff_num_ + 1)
return false;
int key_num = (int)all_key_gnss_.size();
float min_distance = 1000000.0;
float distance = 0.0;
KeyFrame history_key_frame;
KeyFrame current_key_frame = all_key_gnss_.back();
key_frame_index = -1;
for (int i = 0; i < key_num - 1; ++i) {
//# 过于小的闭环没有意义
//所以当前帧的相似帧只在diff_num_帧之前找
if (key_num - i < diff_num_)
break;
//当前关键帧与历史帧(先验信息) 的比较
history_key_frame = all_key_gnss_.at(i);
distance = fabs(current_key_frame.pose(0,3) - history_key_frame.pose(0,3)) +
fabs(current_key_frame.pose(1,3) - history_key_frame.pose(1,3)) +
fabs(current_key_frame.pose(2,3) - history_key_frame.pose(2,3));
if (distance < min_distance) {
min_distance = distance;
key_frame_index = i;
}
}
//放弃初始的几帧
if (key_frame_index < extend_frame_num_)
return false;
skip_cnt = 0;
//防止回环过于频繁
//成功检测一次闭环后之后路径可能有较长一段仍然是重复的,因为是重复路段,这段闭环检测基本都能成功,
//但不希望同一路段增加太多闭环约束,对其进行稀释
//根据当前最小距离计算重新等待时间
skip_num = (int)min_distance;
//detect_area_检测区域,只有两帧距离小于这个值,才做闭环匹配
if (min_distance > detect_area_) {
skip_num = std::max((int)(min_distance / 2.0), loop_step_);
return false;
} else {
skip_num = loop_step_;
return true;
}
}
CloudRegistration()NDT匹配
bool LoopClosing::CloudRegistration(int key_frame_index) {
// 生成地图
CloudData::CLOUD_PTR map_cloud_ptr(new CloudData::CLOUD());
Eigen::Matrix4f map_pose = Eigen::Matrix4f::Identity();
JointMap(key_frame_index, map_cloud_ptr, map_pose);
// 生成当前scan
CloudData::CLOUD_PTR scan_cloud_ptr(new CloudData::CLOUD());
Eigen::Matrix4f scan_pose = Eigen::Matrix4f::Identity();
JointScan(scan_cloud_ptr, scan_pose);
// 匹配
Eigen::Matrix4f result_pose = Eigen::Matrix4f::Identity();
Registration(map_cloud_ptr, scan_cloud_ptr, scan_pose, result_pose);
// 计算相对位姿
current_loop_pose_.pose = map_pose.inverse() * result_pose;
// 判断是否有效GetFitnessScore()返回值越小越好
if (registration_ptr_->GetFitnessScore() > fitness_score_limit_)
return false;
static int loop_close_cnt = 0;
loop_close_cnt ++;
std::cout << "检测到闭环 "<< loop_close_cnt
<< ": 帧" << current_loop_pose_.index0
<< "------>" << "帧" << current_loop_pose_.index1 << std::endl
<< "fitness score: " << registration_ptr_->GetFitnessScore()
<< std::endl << std::endl;
// std::cout << "相对位姿 x y z roll pitch yaw:";
// PrintInfo::PrintPose("", current_loop_pose_.pose);
return true;
}
roslaunch lidar_localization mapping.launch
//受限于电脑性能,bag包的播放频率是正常频率的0.5倍(根据自己实际情况调整)
rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 0.5
启动后的参数信息:
rivz实时显示(第二张图为bag播放完进行手动优化后的结果):
手动优化(将最后未达到优化数量的位姿进行优化,可选项):
rosservice call /optimize_map
保存地图:
rosservice call /save_map
与rviz中显示的三条线对应,在trajectory中保存了三个文件:
橙色线–gnss位姿–ground_truth.txt
红色线–里程计位姿–laser_odom.txt
绿色线–优化后的位姿–optimized.txt
将ground_truth.txt,laser_odom.txt和ground_truth.txt,optimized.txt做两次整体评估:
ground_truth.txt,laser_odom.txt的比较:
evo_ape kitti ground_truth.txt laser_odom.txt -r full --plot --plot_mode xyz
ground_truth.txt,optimized.txt的比较:
evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz
结论:
显然,优化后的位姿与gnss的先验信息比较接近(当然要接近~毕竟我们用它来优化我们的里程计位姿 啊,回环的寻找相似帧也用的gnss信息),其实从riz也能看出来,在优化后橙色线和绿色线基本重合.
注意:其实这样的分析并不够严谨,因为优化的组成是所有关键帧顶点,与gnss的先验信息单边,再与回环检测的双边所构成的全局优化,这里的结论也应该是对全局优化的效果体现。
github链接: SC-LeGO-LOAM.
scan context论文链接: Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map.
我现在也没看过这个框架的完整源码,scan context方法实现主要在Scancontext.cpp中。将点云转换为sc矩阵的接口函数为makeAndSaveScancontextAndKeys(),由saveKeyFramesAndFactor()函数调用实现.而回环的检测则单独拉了一个1hz的线程loopClosureThread,看起来和LeGO-LOAM很相似,线程一直运行performLoopClosure()函数,接着,performLoopClosure()函数里有detectLoopClosure()函数.不同的是LeGO-LOAM使用的是ICP匹配回环,然而SC-LeGO-LOAM在detectLoopClosure()里面调用了detectLoopClosureID()实现了scan context方法的回环.
SC矩阵生成的接口函数makeAndSaveScancontextAndKeys()
void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
//生成scancontext
Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
//得到每个环ring的最大高度平均值 ---> sc矩阵每行数据的平均值 20*1的矩阵
Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
// 每个轴向sector的最大高度平均值 ------>sc矩阵每列数据的平均值
//目的 降维匹配 减少计算量
Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
//将eigen的矩阵转换为vector
//列的平移对应位姿的旋转 保证匹配的旋转不变性
std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
//保存数据
polarcontexts_.push_back( sc );
polarcontext_invkeys_.push_back( ringkey );
polarcontext_vkeys_.push_back( sectorkey );
polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
// cout <
} // SCManager::makeAndSaveScancontextAndKeys
由一帧点云得到sc矩阵makeScancontext()
MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
TicToc t_making_desc;
//激光点数
int num_pts_scan_down = _scan_down.points.size();
// main
const int NO_POINT = -1000;
//构造Scancontext矩阵:环数PC_NUM_RING(20)*扇形数PC_NUM_SECTOR(60)
//初值-1000*1 ones()函数:设置全一矩阵
MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
SCPointType pt;
float azim_angle, azim_range; // wihtin 2d plane
int ring_idx, sctor_idx;
//遍历激光点
for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
{
pt.x = _scan_down.points[pt_idx].x;
pt.y = _scan_down.points[pt_idx].y;
//添加雷达高度 对所有激光点都操作 使其高度大于0 (可能跟数据集有关)
pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
// xyz to ring, sector
//距离
azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
//角度
azim_angle = xy2theta(pt.x, pt.y);
// if range is out of roi, pass
//不考虑太远处的激光点
if( azim_range > PC_MAX_RADIUS )
continue;
//看这个激光点落在矩阵的那个位置
ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );
// taking maximum z
//矩阵内储存该划分的三维空间里所有点的Z的最大值
if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
}
// reset no points to zero (for cosine dist later)
//遍历矩阵,找到所有划分空间里没有激光点的位置 赋值0
for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
if( desc(row_idx, col_idx) == NO_POINT )
desc(row_idx, col_idx) = 0;
t_making_desc.toc("PolarContext making");
return desc;
} // SCManager::makeScancontext
计算两个SC矩阵的列偏移量和距离distanceBtnScanContext()
std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
{
// 1. fast align using variant key (not in original IROS18)
//计算两帧的列向量均值 1*60
MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
//计算列向量的偏移
int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
//取上面得到的初始偏移值argmin_vkey_shift 左右各1/2 SC的列数*SEARCH_RATIO(0.1 )作为搜索空间
const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range
std::vector<int> shift_idx_search_space { argmin_vkey_shift };
for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
{
shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
}
//将偏移量搜索空间排序
std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
// 2. fast columnwise diff
int argmin_shift = 0;
double min_sc_dist = 10000000;
for ( int num_shift: shift_idx_search_space )
{
//将一个SC按偏移量右移
MatrixXd sc2_shifted = circshift(_sc2, num_shift);
//计算两个SC的距离
double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
//找到距离最小的偏移量 以及最小的偏移距离
if( cur_sc_dist < min_sc_dist )
{
argmin_shift = num_shift;
min_sc_dist = cur_sc_dist;
}
}
return make_pair(min_sc_dist, argmin_shift);
} // distanceBtnScanContext
//计算两个sc矩阵的列偏移量的实验函数fastAlignUsingVkey()
int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
{
int argmin_vkey_shift = 0;
double min_veky_diff_norm = 10000000;
for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
{
//将_vkey2矩阵的循环右移shift_idx位
MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
//两矩阵之间相减 如果相似的话,差值应该比较小
MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
double cur_diff_norm = vkey_diff.norm();
//得到两帧的最小偏移
if( cur_diff_norm < min_veky_diff_norm )
{
argmin_vkey_shift = shift_idx;
min_veky_diff_norm = cur_diff_norm;
}
}
return argmin_vkey_shift;
} // fastAlignUsingVkey
源文件的移植
将SC-LeGO-LOAM中涉及到的4个头文件(如图)复制到include/lidar_localization/mapping/loop_closing文件夹下
将Scancontext.cpp同样放到src的loop_closing下面
实现接口
修改复制文件所包含的头文件路径,然后在LoopClosing类中定义一个SCManager类的指针和回环实现的接口函数DetectNearestKeyFrameByScanContext():
bool DetectNearestKeyFrameByScanContext(int& key_frame_index);
std::shared_ptr<SCManager> scmanager_ptr_;
bool use_scancontext_=false;//是否使用ssc方法进行闭环检测
修改loop_closing.cpp中的回环检测函数
if (use_scancontext_)
{
if (!DetectNearestKeyFrameByScanContext(key_frame_index))
return false;
}
else
{
if (!DetectNearestKeyFrame(key_frame_index))
return false;
}
DetectNearestKeyFrameByScanContext()的具体实现如下:
bool LoopClosing::DetectNearestKeyFrameByScanContext(int& key_frame_index)
{
//将当前帧点云存储成SC矩阵
pcl::PointCloud<SCPointType> current_point_cloud;
std::string file_path = key_frames_path_ + "/key_frame_" +std::to_string(all_key_frames_.back().index) + ".pcd";
pcl::io::loadPCDFile(file_path, current_point_cloud);
scmanager_ptr_->makeAndSaveScancontextAndKeys(current_point_cloud);
static int skip_cnt = 0;
static int skip_num = loop_step_;
//并不是每一次关键帧进来都做回环检测 skip_num个关键帧才做一次
if (++skip_cnt < skip_num)
return false;
//回环检测
auto loopResult=scmanager_ptr_->detectLoopClosureID();
if (loopResult.first==-1)
{
return false;
}
key_frame_index =loopResult.first;
return true;
}
在进行闭环的特征匹配时也要由于点云位姿的获取改变了,其对应的函数JointMap()和JointScan也要做对应的修改:
bool LoopClosing::JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose) {
//最相似帧的激光点云的索引
current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
//最相似帧的先验位姿
// 合成地图
//将相似帧附近的关键帧合成局部地图
for (int i = key_frame_index - extend_frame_num_; i < key_frame_index + extend_frame_num_; ++i) {
// 原框架bug:key_frame_index=0时 i<0 数组越界报错
//就是很奇怪,这个bug不应该存在啊 loop_step_不是有初值吗 怎么会key_frame_index=0呢?
//添加越界保护!
if (i<0 || i>=all_key_frames_.size())
{
continue;
}
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);
//最相似帧的先验位姿
if(use_scancontext_)
{
map_pose=all_key_frames_.at(i).pose;
pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, map_pose);
}
else
{
//激光点云与gnss先验信息的变换矩阵
map_pose = all_key_gnss_.at(key_frame_index).pose;
Eigen::Matrix4f pose_to_gnss = map_pose * all_key_frames_.at(key_frame_index).pose.inverse();
//将激光点云转换到gnss的统一坐标下
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;
}
map_filter_ptr_->Filter(map_cloud_ptr, map_cloud_ptr);
return true;
}
bool LoopClosing::JointScan(int key_frame_index,CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose) {
if (use_scancontext_)
{
scan_pose=all_key_frames_.at(key_frame_index).pose;
}
else
{
scan_pose = all_key_gnss_.back().pose;
}
current_loop_pose_.index1 = all_key_frames_.back().index;
current_loop_pose_.time = all_key_frames_.back().time;
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);
return true;
}
未加回环的后端优化
use_loop_close: false
运行过程的优化信息:
ground_truth.txt,optimized.txt的比较:
evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz
迷惑行为:未加回环的后端优化居然效果要比加了(第一种方法)回环的效果要好~
使用SC回环的后端优化
use_loop_close: true
ground_truth.txt,optimized.txt的比较:
evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz
说实话这个误差让我有点懵了~~~~,有没有好心人看看我有那里不对吗
NDT匹配需要较准确的初始位姿,因此在定位之前需要初始化环节,给出载体的初始位姿。
按照难度由低到高,常见的初始化需求有这样几种:
1 已知位姿的初始化
2位置已知而姿态位置的初始化
3位置和姿态均未知的初始化
提供的工程框架中已经给出了位姿初始化功能,但是是在起始位置的,并且是基于已知粗略位姿的。要求实现地图中任意位置的位姿初始化,可以从三种难度等级中任选一种,难度越高,得分越高。
最主要的问题是当进行已知地图的定位时,若定位的起始点不是建图的起点时,无法实现正常的定位功能.
下图为先播放bag包,再启动定位程序的定位效果:
rostopic echo /kitti/oxts/gps/fix
void GNSSData::InitOriginPosition() {
longitude = 8.39200;
latitude = 48.9832;
altitude = 116.793;
geo_converter.Reset(latitude, longitude, altitude);
origin_longitude = longitude;
origin_latitude = latitude;
origin_altitude = altitude;
origin_position_inited = true;
}
未完待续~~~~~~~(2021.05.13)
任意点启动定位程序的定位效果:
分别在100s,200s,300s处进行初始化,然后定位效果如下:
rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 100
rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 200
rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 300
导致代码在后端优化的时候直接内存爆炸,我先试着清理了以下缓存:
sudo sh -c 'echo 1 > /proc/sys/vm/drop_caches'
sudo sh -c 'echo 2 > /proc/sys/vm/drop_caches'
sudo sh -c 'echo 3 > /proc/sys/vm/drop_caches'
发现效果不大,在运行代码的时候,我一直使用top指令(打开终端,输入top)监视着内存和cpu,因为缓存是在代码运行时产生的.于是使用free -m 查询内存,发现没有swap区没有分配~
参考链接: 如何在Ubuntu 16.04上增加Swap分区
手动分配swap区:
1.查看电脑的硬盘信息
df -h
2.在你选择的地方创建一个20G(一般是你系统内存的1.5倍以上)的swap区,从"/dev/zero"文件读出内容并存到你的文件中,并设这
sudo dd if=/dev/zero of=在你电脑选择一个空间较大的位置/swapfile bs=1M count=20k
sudo mkswap 在你电脑选择一个空间较大的位置/swapfile
3.开启或关闭swap区
sudo swapon swapfile文件位置
sudo swapoff swapfile文件位置
4.开机设置
编辑 /etc/fstab ,加入如下一行:
swapfile文件位置 none swap defaults 0 0
回环检测的拼接地图JointMap()函数
bool LoopClosing::JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose) {
//最相似帧的先验位姿
map_pose = all_key_gnss_.at(key_frame_index).pose;
//最相似帧的激光点云的索引
current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
// 合成地图
//激光点云与gnss先验信息的变换矩阵
Eigen::Matrix4f pose_to_gnss = map_pose * all_key_frames_.at(key_frame_index).pose.inverse();
//将相似帧附近的关键帧合成局部地图
for (int i = key_frame_index - extend_frame_num_; i < key_frame_index + extend_frame_num_; ++i) {
// 原框架bug:key_frame_index=0时 i<0 数组越界报错
//就是很奇怪,这个bug不应该存在啊 loop_step_不是有初值吗 怎么会key_frame_index=0呢?
//试着添加下面的if判断,添加越界保护,错误消失
if (i<0 || i>=all_key_frames_.size())
{
continue;
}
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);
//将激光点云转换到gnss的统一坐标下
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;
}
map_filter_ptr_->Filter(map_cloud_ptr, map_cloud_ptr);
return true;
}
这个问题困扰了我两到三天,我甚至一度想要放弃移植了,主要是这个问题没有显示具体的错误,而是一个提示recipe for target “xxx/scancontext.cpp.o” failed(我每次一看到这种错误就头疼)
我做了很多的尝试,但是问题应该是出现在scancontext.cpp,于是又从github上重新下载了源码,但还是爆出相同的错误,折腾了好几天之后,我回顾之前写的关于 智能指针.的博客,想到是不是编译器和c++版本的问题,于是我着重看了下源码,注意c++高版本支持的特性,果然发现了一丝端倪。在detectLoopClosureID函数中,有如下一行:
polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
显然,make_unique这是c++14以后才支持的写法,于是我对源码做了如下改动:
polarcontext_tree_.reset(new InvKeyTree(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ));
重新catkin_make以后错误消失,编译成功~~~~~~~
using SCPointType = pcl::PointXYZI
在它的注释里也说到这种类型是没有intensity信息的,所以为了和代码框架的点云匹配,修改为:
using SCPointType = pcl::PointXYZ