2D SLAM，回环检测与回环修正
-
多分辨率的回环检测
-
基于子地图的回环修正
- 连续约束，相邻submap之间的约束
- 回环约束，loop_constraints_ 之间的约束
-
首先子地图更新时，对 loop_closing_ 进行更新
// 当前submap作为历史地图放入loop closing
if (loop_closing_) {
loop_closing_->AddFinishedSubmap(current_submap_);
}
void LoopClosing::AddFinishedSubmap(std::shared_ptr<Submap> submap) {
auto mr_field = std::make_shared<MRLikelihoodField>();
// 设置多分辨率似然场的pose为子地图的Tws
mr_field->SetPose(submap->GetPose());
// 生成多分辨率似然场
mr_field->SetFieldImageFromOccuMap(submap->GetOccuMap().GetOccupancyGrid());
submap_to_field_.emplace(submap, mr_field);
}
if (loop_closing_) {
loop_closing_->AddNewSubmap(current_submap_);
}
void LoopClosing::AddNewSubmap(std::shared_ptr<Submap> submap) {
submaps_.emplace(submap->GetId(), submap);
last_submap_id_ = submap->GetId();
}
/**
* 单线程的回环检测模块
* 基于里程计估计的位姿进行回环检测，如果检测成功，则更新各submap的pose
*
* 首先调用 DetectLoopCandidates 检测历史地图中可能的回环
* 然后用MultiResolutionMatching进行配准
* 如果配准成功，建立一个pose graph进行优化
* 图优化也可能认为回环是错误的，进而删掉这个回环
*/
class LoopClosing {
public:
/// 一个回环约束
struct LoopConstraints {
LoopConstraints(size_t id1, size_t id2, const SE2& T12) : id_submap1_(id1), id_submap2_(id2), T12_(T12) {}
size_t id_submap1_ = 0;
size_t id_submap2_ = 0;
SE2 T12_; // 相对pose
bool valid_ = true;
};
LoopClosing() { debug_fout_.open("./data/ch6/loops.txt"); }
/// 添加最近的submap，这个submap可能正在构建中
void AddNewSubmap(std::shared_ptr<Submap> submap);
/// 添加一个已经建完的submap，需要在AddNewSubmap函数之后调用
void AddFinishedSubmap(std::shared_ptr<Submap> submap);
/// 为新的frame进行回环检测，更新它的pose和submap的pose
void AddNewFrame(std::shared_ptr<Frame> frame);
/// 获取submap之间的回环检测
std::map<std::pair<size_t, size_t>, LoopConstraints> GetLoops() const { return loop_constraints_; }
bool HasNewLoops() const { return has_new_loops_; }
private:
/// 检测当前帧与历史地图可能存在的回环
bool DetectLoopCandidates();
/// 将当前帧与历史submap进行匹配
void MatchInHistorySubmaps();
/// 进行submap间的pose graph优化
void Optimize();
std::shared_ptr<Frame> current_frame_ = nullptr;
size_t last_submap_id_ = 0; // 最新一个submap的id
std::map<size_t, std::shared_ptr<Submap>> submaps_; // 所有的submaps
// submap到mr field之间的对应关系
std::map<std::shared_ptr<Submap>, std::shared_ptr<MRLikelihoodField>> submap_to_field_;
std::vector<size_t> current_candidates_; // 可能的回环检测点
std::map<std::pair<size_t, size_t>, LoopConstraints> loop_constraints_; // 回环约束, 以被约束的两个submap为索引
bool has_new_loops_ = false;
std::ofstream debug_fout_; // 调试输出
/// 参数
inline static constexpr float candidate_distance_th_ = 15.0; // candidate frame与submap中心之间的距离
inline static constexpr int submap_gap_ = 1; // 当前scan与最近submap编号上的差异
inline static constexpr double loop_rk_delta_ = 1.0; // 回环检测的robust kernel 阈值
};
回环检测与修正流程
- 初始化 loop_closing
// 初始化mapping
if (mapping.Init(FLAGS_with_loop_closing) == false) {
return -1;
}
bool Mapping2D::Init(bool with_loop_closing) {
keyframe_id_ = 0;
// 构造子地图
current_submap_ = std::make_shared<Submap>(SE2());
all_submaps_.emplace_back(current_submap_);
if (with_loop_closing) {
loop_closing_ = std::make_shared<LoopClosing>();
loop_closing_->AddNewSubmap(current_submap_);
}
return true;
}
- mapping.ProcessScan(scan)过程中
- 关键帧出现时进行回环检测与修正
// 处理回环检测
if (loop_closing_) {
loop_closing_->AddNewFrame(current_frame_);
}
void LoopClosing::AddNewFrame(std::shared_ptr<Frame> frame) {
current_frame_ = frame;
// 根据一定要求与条件，检测历史地图中可能的回环
// 并将满足条件的历史子地图索引加入current_candidates_
if (!DetectLoopCandidates()) {
return;
}
// 根据历史子地图 current_candidates_
// T_S1_C 往 current_candidates_ 的多分辨率似然场上配准
// 配准成功计算 T_S1_S2 ，并加入 loop_constraints_
MatchInHistorySubmaps();
if (has_new_loops_) {
Optimize();
}
}
bool LoopClosing::DetectLoopCandidates() {
// 要求当前关键帧与历史submap有一定间隔
has_new_loops_ = false;
if (last_submap_id_ < submap_gap_) {
std::cout << "last_submap_id_: " << last_submap_id_ << std::endl;
return false;
}
current_candidates_.clear();
for (auto& sp : submaps_) {
if ((last_submap_id_ - sp.first) <= submap_gap_) {
// 不检查最近的几个submap
std::cout << "check last_submap_id_: " << last_submap_id_ << "sp.first: " << sp.first << std::endl;
continue;
}
// 如果这个submap和历史submap已经存在有效的关联，也忽略之
auto hist_iter = loop_constraints_.find(std::pair<size_t, size_t>(sp.first, last_submap_id_));
if (hist_iter != loop_constraints_.end() && hist_iter->second.valid_) {
continue;
}
Vec2d center = sp.second->GetPose().translation(); // 当前历史子地图中心的世界系坐标
Vec2d frame_pos = current_frame_->pose_.translation(); // 当前帧的世界系坐标
double dis = (center - frame_pos).norm();
// 离得稍微近一些才检查，这样才会有更多scan重合点
if (dis < candidate_distance_th_) {
// 如果这个frame离submap中心差距小于阈值，则检查
LOG(INFO) << "taking " << current_frame_->keyframe_id_ << " with " << sp.first
<< ", last submap id: " << last_submap_id_;
current_candidates_.emplace_back(sp.first);
}
}
return !current_candidates_.empty();
}
void LoopClosing::MatchInHistorySubmaps() {
// 我们先把要检查的scan, pose和submap存到离线文件, 把mr match调完了再实际上线
// current_frame_->Dump("./data/ch6/frame_" + std::to_string(current_frame_->id_) + ".txt");
for (const size_t& can : current_candidates_) {
// 获取当前历史子地图的多分辨率似然场
// submap_to_field_ 是在扩充子地图时添加的
// 设置被配准的scan
auto mr = submap_to_field_.at(submaps_[can]);
mr->SetSourceScan(current_frame_->scan_);
auto submap = submaps_[can];
// T_S1_C = T_S1_W * T_W_C
SE2 pose_in_target_submap = submap->GetPose().inverse() * current_frame_->pose_;
// T_S1_C 往历史子地图的多分辨率似然场上配准
// 匹配的scan点可能不够多
// 在 AlignG2O 中计算卡方阈值进行判断配准是否成功
if (mr->AlignG2O(pose_in_target_submap)) {
// set constraints from current submap to target submap
// T_S1_S2 = T_S1_C * T_C_W * T_W_S2
SE2 T_this_cur =
pose_in_target_submap * current_frame_->pose_.inverse() * submaps_[last_submap_id_]->GetPose();
// 加一个回环(历史子地图索引， 当前帧所在子地图索引， T_S1_S2)
loop_constraints_.emplace(std::pair<size_t, size_t>(can, last_submap_id_),
LoopConstraints(can, last_submap_id_, T_this_cur));
LOG(INFO) << "adding loop from submap " << can << " to " << last_submap_id_;
/// 在历史子地图s1上，可视化显示，pose_in_target_submap显示在历史子地图上的xy位置
auto occu_image = submap->GetOccuMap().GetOccupancyGridBlackWhite();
Visualize2DScan(current_frame_->scan_, pose_in_target_submap, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
SE2());
cv::putText(occu_image, "loop submap " + std::to_string(submap->GetId()), cv::Point2f(20, 20),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
cv::putText(occu_image, "keyframes " + std::to_string(submap->NumFrames()), cv::Point2f(20, 50),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
cv::imshow("loop closure", occu_image);
has_new_loops_ = true;
}
debug_fout_ << current_frame_->id_ << " " << can << " " << submaps_[can]->GetPose().translation().x() << " "
<< submaps_[can]->GetPose().translation().y() << " " << submaps_[can]->GetPose().so2().log()
<< std::endl;
}
current_candidates_.clear();
}
void LoopClosing::Optimize() {
// pose graph optimization
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
for (auto& sp : submaps_) {
auto* v = new VertexSE2();
v->setId(sp.first);
v->setEstimate(sp.second->GetPose());
optimizer.addVertex(v);
}
/// 连续约束，相邻submap之间的约束
for (int i = 0; i < last_submap_id_; ++i) {
auto first_submap = submaps_[i];
auto next_submap = submaps_[i + 1];
EdgeSE2* e = new EdgeSE2();
e->setVertex(0, optimizer.vertex(i));
e->setVertex(1, optimizer.vertex(i + 1));
// T_S1_S2 = T_S1_W * T_W_S2
e->setMeasurement(first_submap->GetPose().inverse() * next_submap->GetPose());
// 信息矩阵是观测的协方差矩阵的逆，它表示了观测的精度
// 一个大的信息矩阵表示这个观测的不确定性很小，即这个观测非常精确。
e->setInformation(Mat3d::Identity() * 1e4);
optimizer.addEdge(e);
}
/// 回环约束，loop_constraints_ 之间的约束
std::map<std::pair<size_t, size_t>, EdgeSE2*> loop_edges;
for (auto& c : loop_constraints_) {
if (!c.second.valid_) {
continue;
}
auto first_submap = submaps_[c.first.first];
auto second_submap = submaps_[c.first.second];
EdgeSE2* e = new EdgeSE2();
e->setVertex(0, optimizer.vertex(first_submap->GetId()));
e->setVertex(1, optimizer.vertex(second_submap->GetId()));
e->setMeasurement(c.second.T12_);
// 信息矩阵变小，说明观测不确定性较大，即这个观测可能不太精确
e->setInformation(Mat3d::Identity());
auto rk = new g2o::RobustKernelCauchy;
rk->setDelta(loop_rk_delta_);
e->setRobustKernel(rk);
optimizer.addEdge(e);
loop_edges.emplace(c.first, e);
}
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
// validate the loop constraints
int inliers = 0;
for (auto& ep : loop_edges) {
// 卡方误差要小于阈值，才认为是inlier
if (ep.second->chi2() < loop_rk_delta_) {
LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
<< " is correct, chi2: " << ep.second->chi2();
// 认为这个回环边是正确的
// 这样做的目的是让这个正确的循环边在优化过程中有更大的影响力
// 因为没有鲁棒核处理的数据点在优化过程中的权重更大
ep.second->setRobustKernel(nullptr);
loop_constraints_.at(ep.first).valid_ = true;
inliers++;
} else {
// 级别（Level）用于控制优化过程中的迭代次数，级别越高，迭代次数越少。
ep.second->setLevel(1);
LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
<< " is invalid, chi2: " << ep.second->chi2();
loop_constraints_.at(ep.first).valid_ = false;
}
}
optimizer.optimize(5);
for (auto& sp : submaps_) {
// 根据optimizer的结果，更新submap的pose_(Tws)
VertexSE2* v = (VertexSE2*)optimizer.vertex(sp.first);
sp.second->SetPose(v->estimate());
// 更新所属的frame world pose，关键帧 T_S_C 在上面AlignG2O时已经更新了
sp.second->UpdateFramePoseWorld();
}
LOG(INFO) << "loop inliers: " << inliers << "/" << loop_constraints_.size();
// 移除错误的匹配
for (auto iter = loop_constraints_.begin(); iter != loop_constraints_.end();) {
if (!iter->second.valid_) {
iter = loop_constraints_.erase(iter);
} else {
iter++;
}
}
}
PS：EdgeSE2 边的雅可比矩阵采取的是g2o自动求导
class EdgeSE2 : public g2o::BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE2() {}
void computeError() override {
VertexSE2* v1 = (VertexSE2*)_vertices[0];
VertexSE2* v2 = (VertexSE2*)_vertices[1];
// 误差 e = Log(T_w_s1.inverse() * T_s2_w * T_s1_s2.inverse())
_error = (v1->estimate().inverse() * v2->estimate() * measurement().inverse()).log();
}
// TODO jacobian
bool read(std::istream& is) override { return true; }
bool write(std::ostream& os) const override { return true; }
private:
};
297
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
