0. 简介
相比于传统的回环检测方法，SC给激光回环带来了更多的可能性，这里我们将会对SC进行解析，也顺便为之前的两篇文章填坑了。LEGO-LOAM改进思路以及代码， SC-LEGO-LOAM 扩展以及深度解析。Scan Context 就包括空间描述子的定义方法和与之对应的匹配算法。并提供了高效的 bin 编码函数，同时这种编码对点云的密度和法向的变化不敏感。另外SC还保存点云的内部结构，并使用一种有效的两阶段匹配算法，相比于其他空间描述子而言性能更好。
1. Scan Context 论文相关
基于 Scan Context 的位置识别方案整体流程如下图所示：
大概流程为：
- 对一帧点云先进行 SC（Scan Context）的计算
- 从该帧点云的 SC 中提取出一个 N 维的向量（和环数一致）用于在 KD 树中搜索相近的关键帧
- 将搜索得到的参考帧的 SC 和待匹配的当前帧进行比较，如果比较得分高于一定阈值则认为找到回环
1.1 Scan Context 计算
计算 SC 的主要过程为：
- 点云划分
将一帧 3D 点云按照传感器坐标系中的方位角和半径均匀划分不同的 bin，如上图所示，从方位角上看点云被划分成 $N_s$ 个扇面（Sector），从半径反向看，点云被划分成 $N_r$个环（Rings），每个扇面和环相交的部分为一个 bin。每个扇面和环的宽度（分辨率）可以从点云的最大检测距离和扇面/环数量计算得到。
从这种划分方式不难发现，距离较远的 Bin 相比于距离较近的 Bin 会稍微宽一点。这样划分的好处是可以自动对不同距离的点云密度进行动态调节。对于距离较近的地方通常点云密度会高一点，因此我们的 Bin 取窄一点，而相反，对于较远的地方，点云会比较稀疏。因此 Bin 取宽一点可以容纳更多点。
将每个环展开可以得到一个 $N_r\times N_s$ 的二维图像，每个像素点 $P_{ij}$ 是第 $i$ 个环第 $j$ 个扇面对应的 Bin，如上图 b 中所示。这就是 SC 的表现形式，因此下一步要做的就是每个 Bin 分配一个值 $\phi(P_{ij})$。
- 给每个 Bin 分配一个数作为标识，SC 中用每个 Bin 的点中最大的高度作为该 Bin 的标识，对于没有任何点的 Bin 则用 0 作为标识。即：$\phi(P_{ij}) = \max_{\boldsymbol{p}\in P_{ij}}z(\boldsymbol{p})$
从上图中不难看到，虽然 SC 在空间上来看是一个圆，应该有旋转不变性。但我们按照 $0 - 2\pi$ 展开成 2D 图像后，SC 对 Lidar 的朝向就变得敏感了。朝向稍微偏几度可能整体图像就有比较大的平移从而不相似了，为了解决这个问题，作者对图像进行 $N_{trans} = 8$ 次平移，通过这种方法来包含 Lidar 在不同朝向下的 SC。
1.2 $N_r$（Rings）的快速搜索算法
目前主流方法中，在搜索相似帧中主要有三个主要流程：计算相似性、最近邻搜索、稀疏性优化。SC 方案中将计算相似得分和最近邻搜索结合成一步，有效降低了搜索时间。
由于，SC 中的距离计算相较于其他全局描述子较重，因此作者通过引入 Ring Key 提供了一个两阶段的层次搜索算法。Ring Key 是从 SC 中提取出来的一个旋转不变性的描述子。对 SC 中的每一行$r$ 使用一个环编码函数 (Ring encoding function) $\psi$ 来分配一个值作为标识。然后每个环的值组合成一个 $N_r$维的向量作为 Ring Key。$\psi$ 为：
\psi(r_i) = \frac{||r_i||_0}{N_s}
简单来说，Ring Key 的计算方式，计算每个环（行）的密度（有值的像素除以总像素数），然后把不同环的密度组合在一起。显然这种方式提供了旋转不变性。因为无论 Lidar 怎么转只会造成图像平移，对每一行的密度不会有影响。
1.3 Scan Context 的相似性比较
给定两个 SC，需要有一个方式来判断这两个地方的相似性。设 $I^q, I^c$为待比较的两帧点云的 SC。论文中用的方法主要是按列进行比较，对两个 SC 中的每一对对应的列向量（Column Vector）计算出一个距离，对一个列的距离方式维计算两个列向量之间的 cos 距离，整个 SC 的距离用各列的距离之和来表示。具体计算如下所示：
d(I^q, I^c) = \frac{1}{N_s}\sum_{j = 1}^{N_s}\left(1 - \frac{c^q_jc^c_j}{||c^q_j||||c^c_j||}\right)
这种距离计算方式对动态物体比较有效，因为考虑整个 Sector，而不是集中在单个 Bin 的影响。但同样的，这种距离方式对雷达的视角一致要求很高，哪怕在同一个位置雷达视角有一点点不同，可能计算出来的距离都会差别很大。为了解决这个问题，论文中使用的方式进行暴力匹配各个列组合的可能性（对其中一帧进行 $\frac{2\pi}{N_s}$ 次小旋转进行匹配），找出距离最小的组合。
2. 基于SC的回环检测
回环检测的大致流程是对每一个关键帧，在检测器中加入所需的信息，然后进行一次检测，获得当前关键帧对应的历史关键帧索引。对于激光建图来说，所需的信息无外乎是当前帧的点云以及对应的位姿估计。
class Detector {
public:
Detector() = default; //让编译器自动为我们生成函数体
void DistanceDetector(const YAML::Node& node);
void ScanContextDetector(const YAML::Node& node);
void addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) override;
bool DetectNearestKeyFrame(int& key_frame_index) override;
bool SCDetectNearestKeyFrame(int& key_frame_index) override;
private:
int loop_step_ = 5;
int diff_num_ = 10.0;
int detect_area_ = 10.0;
std::deque<KeyFrame> historical_key_frames;
SCManager sc_manager_;
};
void Detector::ScanContextDetector(const YAML::Node& node) {
unsigned int num_exclude_recent = node["num_exclude_recent"].as<unsigned int>();
double search_ratio = node["search_ratio"].as<double>();
double sc_dist_thres = node["sc_dist_thres"].as<double>();
std::cout << "Scan context 使用的参数为："
<< "num_exclude_recent: " << num_exclude_recent << ", search_ratio: " << search_ratio
<< ", sc_dist_thres: " << sc_dist_thres << std::endl;
sc_manager_.setParameters(num_exclude_recent, search_ratio, sc_dist_thres);
}
void Detector::DistanceDetector(const YAML::Node& node) {
loop_step_ = node["loop_step"].as<int>();
diff_num_ = node["diff_num"].as<int>();
detect_area_ = node["detect_area"].as<float>();
}
void Detector::addLatestKeyFrame(const KeyFrame& key_frame, const CloudData::Cloud_Ptr& cloud_data) {
historical_key_frames.push_back(key_frame);
sc_manager_.makeAndSaveScancontextAndKeys(*cloud_data);
}
bool Detector::SCDetectNearestKeyFrame(int& key_frame_index) {
auto detect_result = sc_manager_.detectLoopClosureID(); // first: nn index, second: yaw diff
if (detect_result.first == -1) return false;
key_frame_index = detect_result.first;
return true;
}
bool Detector::DetectNearestKeyFrame(int& key_frame_index) {
static int skip_cnt = 0;
static int skip_num = loop_step_;
if (++skip_cnt < skip_num) return false;
if ((int)historical_key_frames.size() < diff_num_ + 1) return false;
int key_num = (int)historical_key_frames.size();
float min_distance = 1000000.0;
float distance = 0.0;
KeyFrame history_key_frame;
KeyFrame current_key_frame = historical_key_frames.back();
key_frame_index = -1;
for (int i = 0; i < key_num - 1; ++i) {
if (key_num - i < diff_num_) break;
history_key_frame = historical_key_frames.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;
}
}
skip_cnt = 0;
skip_num = (int)min_distance;
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;
}
}
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);
// 从硬盘中读取关键帧点云
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);
static Timer loop_closure_timer("Loop Closure");
loop_closure_timer.tic();
int key_frame_index = 0;
if (search_criteria_ == SearchCriteria::Distance_GNSS) {
// 基于距离搜索距离较近的关键帧
loop_clousre_detector_ptr_->addLatestKeyFrame(key_gnss, scan_cloud_ptr);
} else if (search_criteria_ == SearchCriteria::Distance_Odom) {
// 基于里程计位置估计搜索较近的关键帧
loop_clousre_detector_ptr_->addLatestKeyFrame(key_frame, scan_cloud_ptr);
} else if (search_criteria_ == SearchCriteria::ScanContext) {
// 将下采样后的点云传入 sc manager 构建并存储 scan context
loop_clousre_detector_ptr_->addLatestKeyFrame(key_frame, scan_cloud_ptr);
}
has_new_loop_pose_ = loop_clousre_detector_ptr_->SCDetectNearestKeyFrame(key_frame_index);
has_new_loop_pose_ = loop_clousre_detector_ptr_->SCDetectNearestKeyFrame(key_frame_index);
loop_closure_timer.toc();
if (!has_new_loop_pose_ && !has_dis_loop_pose_ ) return false;
// 初始化回环信息（此时置信度设为 false）
current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
current_loop_pose_.index1 = all_key_frames_.back().index;
current_loop_pose_.time = all_key_frames_.back().time;
current_loop_pose_.confidence = false;
std::cout << "检测到闭环 "
<< ": 帧" << current_loop_pose_.index0 << "------>"
<< "帧" << current_loop_pose_.index1 << std::endl;
if (key_frame_index < extend_frame_num_) {
std::cout << "匹配失败！\n";
return false;
}
if (CloudRegistration(key_frame_index)) {
current_loop_pose_.confidence = true;
}
LOG(INFO) << loop_closure_timer;
return true;
}
3 参考链接
https://zhuanlan.zhihu.com/p/393353116
https://blog.csdn.net/wyang9x/article/details/117446642
https://www.cnblogs.com/chenlinchong/p/13154874.html
评论（0）
您还未登录，请登录后发表或查看评论