《自动驾驶与机器人中的SLAM技术：从理论到实践》笔记——ch6（2）
在进行扫描匹配之后，得到了两个扫描数据之间的相对运动。这个过程相当于SLAM中的定位问题，得到位置之后我们就要进行建图工作。对于建图问题因为物体的移动数据是进行变化，当这些数据在一个固定大小的地图上时，是将所有的扫描点放入构成地图，还是应该设置一些相互覆盖、刷新的策略呢，6.3就是来学习地图的管理模式，。首先是占据栅格地图，是以栅格的形式（或者以图像的形式）存储占据概率的地图。
·
6.3占据栅格地图
6.3.1占据栅格地图的原理
在进行扫描匹配之后，得到了两个扫描数据之间的相对运动。这个过程相当于SLAM中的定位问题，得到位置之后我们就要进行建图工作。对于建图问题因为物体的移动数据是进行变化，当这些数据在一个固定大小的地图上时，是将所有的扫描点放入构成地图，还是应该设置一些相互覆盖、刷新的策略呢，6.3就是来学习地图的管理模式，基于占据栅格地图和子地图的管理模式。
首先是占据栅格地图，是以栅格的形式（或者以图像的形式）存储占据概率的地图。栅格是一种非常简单的二维地图形式，它把地图分为许多平面的小格，然后每个小格子内存储一些自定义的信息，如果存储的是障碍物信息，就称为障碍物栅格地图，可以用于路径规划，若存储语义信息，称为语义栅格。栅格地图和图像关联，每个栅格可以和图像像素一一对应。
在进行路径规划任务中，我们往往关注栅格的可通行情况，所以只需表达出每个栅格是否有物体占据即可，有没有物体占据呢，我们将其归为一个概率信息，这个概率大小为多次观察到某个障碍物在此栅格中的概率。
占据栅格地图应该满足以下几个描述：1.以栅格的形式存储每个格子被障碍物占据的概率，此数为浮点数。2.栅格具有一定的分辨率，且通常是稠密的。3.占据概率会随着观测而发生改变。二维激光雷达的占据栅格只能表明在这个高度下是否存在障碍物。
在栅格化过程中，可以选择先计算一个固定大小的模板区域，对其进行栅格化。
6.3.2基于Bresenham算法的地图生成
Bresenham算法是一种对直线进行栅格化的算法，通常用于几何直线的矢量化，它可以完全由整数运算来实现，所以效率高。设地图坐标系下，机器人原点为，某个末端点为
，两者均为整数。我们希望在地图中填充一条从
指向
的直线并将其标记为可通行区域。算法流程如下：
视频讲解：视频讲解-Bresenham 画线算法
void OccupancyMap::BresenhamFilling(const Vec2i& p1, const Vec2i& p2) {
int dx = p2.x() - p1.x();
int dy = p2.y() - p1.y();
int ux = dx > 0 ? 1 : -1;
int uy = dy > 0 ? 1 : -1;
dx = abs(dx);
dy = abs(dy);
int x = p1.x();
int y = p1.y();
if (dx > dy) {
// 以x为增量
int e = -dx;
for (int i = 0; i < dx; ++i) {
x += ux;
e += 2 * dy;
if (e >= 0) {
y += uy;
e -= 2 * dx;
}
if (Vec2i(x, y) != p2) {
SetPoint(Vec2i(x, y), false);
}
}
} else {
int e = -dy;
for (int i = 0; i < dy; ++i) {
y += uy;
e += 2 * dx;
if (e >= 0) {
x += ux;
e -= 2 * dy;
}
if (Vec2i(x, y) != p2) {
SetPoint(Vec2i(x, y), false);
}
}
}
}6.3.3基于模板的地图生成
如果不使用直线填充算法，或者直线数量和距离明显超过指定区域，那么可以考虑使用模板算法进行填充。模板区域内每个格子的角度和距离都是预先算好的。需要更新栅格地图时，将模板中的每个格子的距离值与激光在该角度下的距离进行比较 。如果模板中的距离小于激光打到的距离，就可以认为该格子是空白的。如果等于激光打到的距离值，则认为该格子被占据。如果大于激光打到的距离值，该格子不做更新。这样，就可以回避对每个激光线进行栅格化的过程。
栅格地图的更新原则上应该按照概率形式计算。由于8位数图像使用0~255的整数来描述灰度。所以，如果一个栅格值为127，则可以认为该格子的状态为未知。每次观测到模型时，让该格子的数值减一；反之加一，同时也要限制格子的最大与最小计数，既能达到更新效果也可节约计算时间。
在高翔老师的文章中，为了与后文保持兼容，需要注意以下几点。
1.此时介绍的是如何对二维数据生成局部似然场，以及如何将这栅格地图合并成一个全局地图。
2.占据上个地图也会和似然场绑定。似然场用栅格地图已经表示障碍物的栅格生成。
3.由于后文会以子地图的形式来管理地图，因此会对每个子地图生成一个对应的占据栅格地图。
如何计算栅格地图中哪些格点应该被观测为障碍物，哪些又被观测为可通行状态。
首先，栅格地图会在构造时生成一个固定大小的模板。预先计算好模板点的距离和夹角：
void OccupancyMap::BuildModel() {
// 遍历x轴方向：从-model_size_到model_size_（包含边界），覆盖正方形左半到右半
for (int x = -model_size_; x <= model_size_; x++) {
// 遍历y轴方向：从-model_size_到model_size_（包含边界），覆盖正方形下半到上半
for (int y = -model_size_; y <= model_size_; y++) {
// 定义2D模型点结构体，存储该采样点的相对坐标和极坐标信息
Model2DPoint pt;
// 存储该点相对于中心的x/y方向离散偏移量（栅格数）
pt.dx_ = x;
pt.dy_ = y;
// 计算该点到中心的实际物理距离：
// 1. 先计算栅格空间的欧几里得距离 sqrt(x²+y²)
// 2. 乘以分辨率倒数（inv_resolution_ = 1/resolution）转换为物理距离（如米）
pt.range_ = sqrt(x * x + y * y) * inv_resolution_;
// 计算该点相对于中心的方位角（弧度）：atan2(y, x)返回[-π, π]范围的角度
pt.angle_ = std::atan2(y, x);
// 角度归一化：将角度限制到[-π, π]（等价于0~2π的另一种表示）
// 注：原注释"limit in 2pi"表述稍欠准确，实际是将大于π的角度转换为负角度，保持范围连续
pt.angle_ = pt.angle_ > M_PI ? pt.angle_ - 2 * M_PI : pt.angle_;
// 将该采样点添加到模型点集末尾
model_.push_back(pt);
}
}
}利用这个模板来更新栅格信息，先将激光雷达扫描数据转到世界坐标系下，然后按照角度进行排序。遍历所有模板栅格，比较模板栅格在某个角度中的距离是否大于激光雷达的距离，再进行栅格的更新操作。
void OccupancyMap::AddLidarFrame(std::shared_ptr<Frame> frame, GridMethod method) {
auto& scan = frame->scan_;
// 此处不能直接使用frame->pose_submap_，因为frame可能来自上一个地图
// 此时frame->pose_submap_还未更新，依旧是frame在上一个地图中的pose
SE2 pose_in_submap = pose_.inverse() * frame->pose_;
float theta = pose_in_submap.so2().log();
has_outside_pts_ = false;
// 先计算末端点所在的网格
std::set<Vec2i, less_vec<2>> endpoints;
for (size_t i = 0; i < scan->ranges.size(); ++i) {
if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
continue;
}
double real_angle = scan->angle_min + i * scan->angle_increment;
double x = scan->ranges[i] * std::cos(real_angle);
double y = scan->ranges[i] * std::sin(real_angle);
endpoints.emplace(World2Image(frame->pose_ * Vec2d(x, y)));
}
if (method == GridMethod::MODEL_POINTS) {
// 遍历模板，生成白色点
std::for_each(std::execution::par_unseq, model_.begin(), model_.end(), [&](const Model2DPoint& pt) {
Vec2i pos_in_image = World2Image(frame->pose_.translation());
Vec2i pw = pos_in_image + Vec2i(pt.dx_, pt.dy_); // submap下
if (pt.range_ < closest_th_) {
// 小距离内认为无物体
SetPoint(pw, false);
return;
}
double angle = pt.angle_ - theta; // 激光系下角度
double range = FindRangeInAngle(angle, scan);
if (range < scan->range_min || range > scan->range_max) {
/// 某方向无测量值时，认为无效
/// 但离机器比较近时，涂白
if (pt.range_ < endpoint_close_th_) {
SetPoint(pw, false);
}
return;
}
if (range > pt.range_ && endpoints.find(pw) == endpoints.end()) {
/// 末端点与车体连线上的点，涂白
SetPoint(pw, false);
}
});
} else {
Vec2i start = World2Image(frame->pose_.translation());
std::for_each(std::execution::par_unseq, endpoints.begin(), endpoints.end(),
[this, &start](const auto& pt) { BresenhamFilling(start, pt); });
}
/// 末端点涂黑
std::for_each(endpoints.begin(), endpoints.end(), [this](const auto& pt) { SetPoint(pt, true); });
}栅格的更新结果直观地体现在“染黑”（占据栅格）或者“染白”（通行栅格）这样的染色问题。在实际工程中要考虑机器人体积、激光雷达的安装位置、是否有人在行走。接下来，不做扫描匹配，单独测试对单个扫描数据进行栅格地图建图的结果。此时,栅格地图只有一个扫描数据，其中大部分栅格的概率值在 0.5 上下，将它进行二值化后显示出来，即只要栅格中的存储值不等于127，就会显示黑色或者白色。
cv::Mat OccupancyMap::GetOccupancyGridBlackWhite() const {
// 创建空白RGB彩色图像：尺寸image_size_×image_size_，像素类型CV_8UC3（8位3通道）
cv::Mat image(image_size_, image_size_, CV_8UC3);
// 遍历占据栅格地图的所有列（x轴方向）
for (int x = 0; x < occupancy_grid_.cols; ++x) {
// 遍历占据栅格地图的所有行（y轴方向）
for (int y = 0; y < occupancy_grid_.rows; ++y) {
// 获取当前栅格的占据值（uchar类型，0~255）
uchar grid_value = occupancy_grid_.at<uchar>(y, x);
// 根据占据值映射为对应颜色：
if (grid_value == 127) {
// 占据值=127 → 灰色：表示未知/未探索的栅格区域
image.at<cv::Vec3b>(y, x) = cv::Vec3b(127, 127, 127);
} else if (grid_value < 127) {
// 占据值<127 → 黑色：表示被占据/有障碍物的栅格区域
image.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
} else if (grid_value > 127) {
// 占据值>127 → 白色：表示空闲/可通行的栅格区域
image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);
}
}
}
// 返回生成的黑白灰可视化图像
return image;
}
模板方法比直线方法计算量大。
6.4子地图
6.4.1子地图的原理
将匹配算法和栅格地图放到一起，把匹配算法和栅格地图放在一起，利用栅格地图的更新机制将匹配好的数据放到一起。进一步，可以把若干个匹配好的结果放在一起，形成一个个子地图(Submap)。子地图是介于单个扫描数据与全局地图之间的数据组织形式。它把若干个扫描数据按时间顺序归到一起，使用起来非常灵活。
实现基于子地图的2DSLAM.
1.一个子地图对应一个似然场和一个栅格地图。
2.总是把当前的扫描数据与当前的子地图进行匹配，得到该扫描数据在当前子地图中的位姿。
3.如果机器人发生移动或转动，则按一定距离和角度取关键帧。
4.如果机器人的移动范围超出了当前子地图，或者当前子地图包含的关键帧超出了一定数量，就建立一个新的子地图。新的子地图以当前帧为中心，它的位姿取（获取的数据与世界坐标系）(地图与世界坐标系)。此时，新地图上面完全没有数据。为了方便后续配准，我们把旧的子地图里最近的一些关键帧复制至新的子地图中。
5.合并每个子地图的栅格地图，就可以得到全局地图
（简单理解为划定一个空间来存储当前的数据，之后空间满了就开始拓展，拓展时以当前数据即为当前帧为中心，得到第二个子地图，重复这个过程到最后，根据中心的位置将子地图拼接成全局地图）
6.4.2子地图的实现
栅格地图和似然场的对象在Submap类的内部，其主要内容为：
/**
* @class Submap
* @brief 子地图核心类，负责管理局部区域的关键帧、占据栅格地图和似然场
*
* 核心设计逻辑：
* 1. 每个子地图有独立的世界位姿 Tws（世界坐标系→子地图坐标系的SE2变换）；
* 2. 子地图关联若干关键帧，关键帧的世界位姿 = Tws × 帧在子地图内的相对位姿；
* 3. 新增关键帧时同步更新占据栅格地图和似然场，支撑扫描匹配；
* 4. 子地图位姿更新后，需重新计算所有关联关键帧的世界位姿。
*/
class Submap {
public:
/**
* @brief 构造函数，初始化子地图位姿并同步初始化栅格地图和似然场
* @param pose 子地图的初始世界位姿 Tws（世界→子地图的SE2变换）
*/
Submap(const SE2& pose) : pose_(pose) {
occu_map_.SetPose(pose_);
field_.SetPose(pose_);
}
/**
* @brief 扫描匹配：将输入帧与当前子地图的似然场/栅格地图匹配，求解帧的最优位姿
* @param frame 待匹配的帧（需包含激光扫描数据和初始位姿估计）
* @return bool 匹配是否成功：true=匹配收敛，false=匹配失败（如无有效匹配点）
*/
bool MatchScan(std::shared_ptr<Frame> frame);
/**
* @brief 将单帧激光扫描数据添加到占据栅格地图，更新栅格占据状态
* @details 核心栅格更新接口，新增关键帧时需先调用此接口更新地图，再调用AddKeyFrame
* @param frame 包含激光扫描数据的帧（需已完成位姿计算）
*/
void AddScanInOccupancyMap(std::shared_ptr<Frame> frame);
/**
* @brief 将帧添加为当前子地图的关键帧
* @details 仅将帧存入关键帧列表，不更新栅格地图/似然场（需提前调用AddScanInOccupancyMap）
* @param frame 待添加的关键帧智能指针（需保证帧在子地图内的相对位姿已计算）
*/
void AddKeyFrame(std::shared_ptr<Frame> frame) { frames_.emplace_back(frame); }
private:
SE2 pose_; ///< 子地图的世界位姿 Tws（世界→子地图的SE2变换）
size_t id_ = 0; ///< 子地图唯一标识ID，默认初始化为0
std::vector<std::shared_ptr<Frame>> frames_; ///< 子地图关联的所有关键帧列表
LikelihoodField field_; ///< 似然场，用于扫描匹配（MatchScan）的位姿求解
OccupancyMap occu_map_; ///< 占据栅格地图，存储局部环境的栅格化占据状态
};对于子地图扫描匹配与栅格地图的更新函数，如下：
/**
* @brief 扫描匹配：将输入帧与当前子地图的似然场匹配，求解帧在子地图中的最优位姿并更新帧的世界位姿
* @details 核心流程：
* 1. 将帧的激光扫描数据设置为似然场的匹配源数据；
* 2. 基于g2o优化器，以帧在子地图中的初始位姿（pose_submap_）为初值，对齐似然场求解最优位姿；
* 3. 根据子地图世界位姿（Tws）和帧在子地图中的最优位姿（Tsc），计算帧的世界位姿（Twc）；
* 4. 注：当前实现默认匹配成功，返回值恒为true（实际工程中需增加匹配收敛性判断）。
* @param frame 待匹配的帧（需包含激光扫描数据scan_和子地图内初始位姿pose_submap_）
* @return bool 匹配结果标识：当前实现恒返回true，实际应用中需根据g2o优化的收敛性/误差阈值返回真实结果
*/
bool Submap::MatchScan(std::shared_ptr<Frame> frame) {
// 设置似然场的匹配源扫描数据为当前帧的激光扫描数据
field_.SetSourceScan(frame->scan_);
// 基于g2o优化框架，以帧在子地图中的初始位姿为初值，对齐似然场求解最优位姿（更新frame->pose_submap_）
field_.AlignG2O(frame->pose_submap_);
// 计算帧的世界位姿：世界→相机 = 世界→子地图 × 子地图→相机（T_w_c = T_w_s * T_s_c）
frame->pose_ = pose_ * frame->pose_submap_;
// 当前实现默认匹配成功，实际需补充收敛性判断（如优化后的误差是否小于阈值）
return true;
}
/**
* @brief 将帧的激光数据添加到占据栅格地图，并同步更新似然场
* @details 核心流程：
* 1. 基于激光帧数据更新占据栅格地图的栅格状态（采用MODEL_POINTS方法建模激光点）；
* 2. 从更新后的占据栅格地图重新生成似然场的场函数图像，保证似然场与栅格地图同步；
* 3. 该函数是子地图地图更新的核心接口，需在AddKeyFrame前调用。
* @param frame 待添加的激光帧（需已完成位姿计算：包含世界位姿pose_或子地图内位姿pose_submap_）
*/
void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {
// 向占据栅格地图添加激光帧，采用MODEL_POINTS方法（基于激光点直接建模栅格占据状态）更新栅格
occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);
// 从更新后的占据栅格地图中提取栅格数据，重新生成似然场的场函数图像（保证似然场与栅格地图一致）
field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());
}外层建图流程：
/**
* @brief 处理单帧激光扫描数据，完成位姿估计、关键帧判定、子地图更新/扩展及可视化
* @details 2D激光SLAM核心处理流程：
* 1. 初始化当前帧，基于上一帧位姿+运动估计设置初始位姿；
* 2. 非第一帧时执行扫描匹配，优化当前帧位姿；
* 3. 判断当前帧是否为关键帧，若是则更新子地图并触发回环检测；
* 4. 检查子地图是否需要扩展（外部点/关键帧数量阈值），若需扩展则新建子地图；
* 5. 可视化占据栅格地图、似然场及全局地图；
* 6. 计算帧间运动估计，更新历史帧指针；
* @param scan 输入的2D激光扫描数据智能指针（包含激光点云、时间戳等信息）
* @return bool 处理是否成功：当前实现恒返回true，实际可扩展错误码/失败场景（如扫描数据为空）
*/
bool Mapping2D::ProcessScan(Scan2d::Ptr scan) {
// 1. 初始化当前帧：创建Frame对象并分配帧ID
current_frame_ = std::make_shared<Frame>(scan);
current_frame_->id_ = frame_id_++;
// 2. 设置当前帧初始位姿（基于上一帧位姿+运动估计）
if (last_frame_) {
// 初始位姿 = 上一帧世界位姿 × 帧间运动估计（运动模型预测）
current_frame_->pose_ = last_frame_->pose_ * motion_guess_;
// 初始子地图内相对位姿继承上一帧（假设帧仍在当前子地图内）
current_frame_->pose_submap_ = last_frame_->pose_submap_;
}
// 3. 扫描匹配：非第一帧时，基于子地图似然场优化当前帧位姿
if (!first_scan_) {
// 第一帧无地图可匹配，跳过；后续帧执行匹配（更新pose_submap_和pose_）
current_submap_->MatchScan(current_frame_);
}
// 标记第一帧处理完成（后续帧均执行扫描匹配）
first_scan_ = false;
// 4. 关键帧判定：基于位姿差阈值判断是否为关键帧
bool is_kf = IsKeyFrame();
if (is_kf) {
// 5. 若是关键帧：添加到子地图、更新栅格地图/似然场
AddKeyFrame();
current_submap_->AddScanInOccupancyMap(current_frame_);
// 6. 回环检测：将新关键帧加入回环检测队列
if (loop_closing_) {
loop_closing_->AddNewFrame(current_frame_);
}
// 7. 子地图扩展判定：存在外部点 或 子地图内关键帧数超过50
if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {
/// 触发条件：帧扫描点超出子地图范围 或 子地图关键帧数量过多（避免单张子地图过大）
ExpandSubmap();
}
}
/// 8. 可视化模块：实时展示子地图占据栅格、似然场及全局地图
// 可视化子地图占据栅格（叠加当前帧激光扫描）
auto occu_image = current_submap_->GetOccuMap().GetOccupancyGridBlackWhite();
Visualize2DScan(current_frame_->scan_, current_frame_->pose_, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
current_submap_->GetPose());
// 绘制子地图ID和关键帧数量文本
cv::putText(occu_image, "submap " + std::to_string(current_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(current_submap_->NumFrames()), cv::Point2f(20, 50),
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
cv::imshow("occupancy map", occu_image);
// 可视化似然场（叠加当前帧激光扫描）
auto field_image = current_submap_->GetLikelihood().GetFieldImage();
Visualize2DScan(current_frame_->scan_, current_frame_->pose_, field_image, Vec3b(0, 0, 255), 1000, 20.0,
current_submap_->GetPose());
cv::imshow("likelihood", field_image);
/// 全局地图可视化：仅关键帧时更新（减少计算量）
if (is_kf) {
cv::imshow("global map", ShowGlobalMap());
}
// 等待10ms刷新可视化窗口（保证实时显示）
cv::waitKey(10);
// 9. 更新帧间运动估计：当前帧与上一帧的相对位姿（用于下一帧初始位姿预测）
if (last_frame_) {
motion_guess_ = last_frame_->pose_.inverse() * current_frame_->pose_;
}
// 10. 更新历史帧指针：当前帧变为上一帧，供下一帧使用
last_frame_ = current_frame_;
return true;
}
/**
* @brief 判断当前帧是否为关键帧
* @details 关键帧判定规则（满足其一则为关键帧）：
* 1. 无历史关键帧（首次判定）；
* 2. 与上一关键帧的位姿差超过位置/角度阈值；
* 关键帧作用：用于子地图构建、回环检测、全局优化，平衡计算量与地图精度。
* @return bool 是关键帧返回true，否则返回false
*/
bool Mapping2D::IsKeyFrame() {
// 规则1：无历史关键帧 → 直接判定为关键帧
if (last_keyframe_ == nullptr) {
return true;
}
// 计算当前帧与上一关键帧的相对位姿：T_kf^{-1} * T_current（上一关键帧→当前帧的变换）
SE2 delta_pose = last_keyframe_->pose_.inverse() * current_frame_->pose_;
// 规则2：位置差超过阈值 或 角度差超过阈值 → 判定为关键帧
if (delta_pose.translation().norm() > keyframe_pos_th_ || fabs(delta_pose.so2().log()) > keyframe_ang_th_) {
return true;
}
// 未满足阈值条件 → 非关键帧
return false;
}
/**
* @brief 将当前帧添加为关键帧，更新子地图和历史关键帧指针
* @details 关键帧注册流程：
* 1. 打印日志记录关键帧ID；
* 2. 为当前帧分配唯一关键帧ID；
* 3. 将关键帧添加到当前子地图的关键帧列表；
* 4. 更新历史关键帧指针为当前帧。
*/
void Mapping2D::AddKeyFrame() {
// 日志输出：记录关键帧添加事件（便于调试/日志分析）
LOG(INFO) << "add keyframe " << keyframe_id_;
// 为当前帧分配关键帧ID并自增
current_frame_->keyframe_id_ = keyframe_id_++;
// 将当前帧添加到当前子地图的关键帧列表
current_submap_->AddKeyFrame(current_frame_);
// 更新历史关键帧指针
last_keyframe_ = current_frame_;
}
/**
* @brief 扩展子地图：创建新子地图替换当前子地图，保证SLAM地图的局部性
* @details 子地图扩展流程（解决单张子地图过大/超出范围问题）：
* 1. 将旧子地图标记为"已完成"，加入回环检测模块；
* 2. 保存旧子地图的栅格图像（调试用）；
* 3. 创建新子地图（初始位姿为当前帧世界位姿）；
* 4. 重置当前帧在新子地图内的相对位姿（归零，作为新子地图的初始帧）；
* 5. 设置新子地图ID，添加当前帧为首个关键帧；
* 6. 复制旧子地图的栅格数据（避免新子地图初始为空）；
* 7. 更新新子地图的栅格地图/似然场；
* 8. 将新子地图加入全局子地图列表，并通知回环检测模块；
* 9. 日志输出新子地图信息（便于调试）。
*/
void Mapping2D::ExpandSubmap() {
// 1. 旧子地图加入回环检测（已完成的子地图参与回环匹配）
if (loop_closing_) {
loop_closing_->AddFinishedSubmap(current_submap_);
}
// 保存旧子地图指针（用于复制栅格数据/调试）
auto last_submap = current_submap_;
// debug：保存旧子地图的栅格图像到本地（便于离线分析）
cv::imwrite("./data/ch6/submap_" + std::to_string(last_submap->GetId()) + ".png",
last_submap->GetOccuMap().GetOccupancyGridBlackWhite());
// 2. 创建新子地图：初始位姿为当前帧的世界位姿
current_submap_ = std::make_shared<Submap>(current_frame_->pose_);
// 重置当前帧在新子地图内的相对位姿（T_s_c = 单位矩阵，作为新子地图的基准帧）
current_frame_->pose_submap_ = SE2();
// 3. 初始化新子地图属性
current_submap_->SetId(++submap_id_); // 设置新子地图唯一ID
current_submap_->AddKeyFrame(current_frame_); // 添加当前帧为新子地图首个关键帧
// 复制旧子地图的栅格数据（避免新子地图无初始数据，提升匹配鲁棒性）
current_submap_->SetOccuFromOtherSubmap(last_submap);
// 4. 更新新子地图的栅格地图和似然场
current_submap_->AddScanInOccupancyMap(current_frame_);
// 将新子地图加入全局子地图列表
all_submaps_.emplace_back(current_submap_);
// 5. 通知回环检测模块：添加新子地图（参与后续回环匹配）
if (loop_closing_) {
loop_closing_->AddNewSubmap(current_submap_);
}
// 日志输出：记录新子地图ID和位姿（便于调试位姿连续性）
LOG(INFO) << "create submap " << current_submap_->GetId()
<< " with pose: " << current_submap_->GetPose().translation().transpose() << ", "
<< current_submap_->GetPose().so2().log();
}总是把当前的扫描数据在当前的子地图中进行匹配。如果机器运动了一段距离，就把当前帧设置为新的关键帧。每个关键帧都会被放到当前的子地图中。如果子地图中的关键帧数量超过预定数目，则添加新的子地图，然后把旧的子地图放到历史数据中。
测试程序：
/**
* @brief 2D激光SLAM主流程：从ROS bag读取激光数据，运行建图模块并保存最终全局地图
* @details 核心流程：
* 1. 初始化ROS bag数据读取器，指定bag文件路径；
* 2. 初始化2D建图器实例；
* 3. 清理历史建图数据（避免旧文件干扰）；
* 4. 初始化建图模块（可选启用回环检测）；
* 5. 注册激光数据回调函数，执行bag数据回放与建图；
* 6. 保存最终全局地图到本地；
* 7. 程序正常退出。
*/
// 1. 初始化ROS bag IO模块：指定待读取的bag文件路径，用于解析激光扫描数据
sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
// 2. 初始化2D建图器核心实例（包含子地图管理、扫描匹配、关键帧判定等逻辑）
sad::Mapping2D mapping;
// 3. 清理历史建图数据目录：删除./data/ch6/下所有文件，避免旧子地图/地图文件干扰本次建图
std::system("rm -rf ./data/ch6/*");
// 4. 初始化建图模块：传入是否启用回环检测的参数，初始化失败则退出程序
if (mapping.Init(FLAGS_with_loop_closing) == false) {
LOG(ERROR) << "Mapping2D init failed!"; // 补充日志：便于定位初始化失败原因
return -1;
}
// 5. 注册2D激光扫描数据回调+执行bag回放：
// - 订阅bag中"/pavo_scan_bottom"话题的2D激光数据；
// - 回调函数：将每帧激光数据传入建图器的ProcessScan接口处理；
// - Go()：启动bag数据回放，阻塞直到所有数据处理完成
rosbag_io.AddScan2DHandle("/pavo_scan_bottom", [&](Scan2d::Ptr scan) {
return mapping.ProcessScan(scan);
}).Go();
// 6. 保存最终全局地图：生成2000x2000分辨率的全局地图，保存到指定路径
cv::imwrite("./data/ch6/global_map.png", mapping.ShowGlobalMap(2000));
// 7. 程序正常退出
return 0;
}测试结果：
我们可以在图中看到当机器运行一段时间后，累计误差会使地图产生明显的重影现象，这个问题将使用回环检测消除这些误差，从而解决这个问题。
6.5 回环检测与闭环
激光 SLAM 建图的基础是逐帧位姿估计,但每一次位姿估计都会存在微小误差（比如激光匹配的噪声、传感器精度限制、运动模型误差）。
-
短距离内，这些误差可以忽略；
-
长距离 / 长时间建图时，误差会累积放大（即「位姿漂移」），最终导致：
-
局部地图看似正常，但全局地图严重变形（比如直走廊建成曲线、闭合空间（房间 / 走廊）无法闭合）；
-
子地图扩展后，新旧子地图错位，全局地图碎片化；
-
地图无法用于后续的导航、路径规划（位置偏差会导致机器人 “迷路”）
回环检测的本质是测机器人是否回到了曾经访问过的区域，并利用这一 “回环约束” 修正全局位姿和地图，从根源上消除累积误差。回环的位姿检测是由累积误差的多少决定的，由于我们之前在做点云数据处理使用到的方法如ICP或似然场均受初值的严重影响，所以我们的回环检测是在原有的ICP或似然场基础上，增加一些针对于较差位姿初值的处理方法，如网格搜索、粒子滤波、分枝定界、金字塔法。
由此进行发问，为什么在构建SLAM地图时，会出现初始位姿值不确定的问题？这个问题的本质是单帧扫描匹配的初始位姿与子地图内的初始相对位置都会不确定。导致初始位姿不确定的核心原因是运动模型/帧间预测的固有误差（1.运动帧估计之间的误差与初始位姿默认为原点导致的）与环境特征值的缺失。
在检测到回环之后，需要对整个地图进行位姿修正。这种修正既可以基于关键帧的位姿来做，也可基于子地图来做（此时是将其当做一个独立的单元来进行处理）。由于地图中子地图数量少于关键帧的数量计算起来比较方便，但是此时子地图内部的畸形没办法配准，这时就要根据场景进行选择。
回环检测的作用体现在：
1.修正位姿漂移，保证地图全局一致性。主要体现在当检测到“当前帧/子地图”与“历史帧/子地图”对应同一个区域，就构成回环约束（一般从位姿出发做配准），其次体现在通过全局优化，基于回环约束调整所有的关键帧/子地图的位姿，来抵消累积位姿误差，之后将扭曲的地图恢复成正常的地图。
2.解决子地图拓展带来的拼接误差。
3.提升建图鲁棒性，抵抗局部匹配错误。
6.5.1多分辨率的回环检测
金字塔式的回环检测方式，也被称为由粗到精的，或者多分辨率的配准方法。金字塔方法既可以用于似然场的配准，也是光流法中的重要手段，从遍历的角度来看，能快速地遍历某个问题的求解范围，能快速找到最优解。由于分枝定界和由粗至精的方法都需要用到多个分辨率下的栅格地图，所以统一归类至多分辨率下的回环检测方法。
本节介绍的多分辨率的似然场匹配法，本质是一个扫描匹配问题加上了初始位姿估计不准确的条件，为了减少初始值不准确带来的影响，可在原有的似然场之外，增加一些其他分辨率的似然场。通常是是在小分辨率的似然场中先进性一次配准，然后把粗配准的结果投影到高分辨率的似然场中，形成一个由粗至精的匹配过程。（补一个小知识：slam中定义为每个栅格对应现实世界的物理长度，所以是分辨率越大的话，越不清楚）
下图直观地显示了多分辨率配准的可视化结果。这里高老师使用了四层似然场，每往上一层，场的图像大小缩了一半。最小分辨率下的似然场里的障碍物形状难以看清，但是每个障碍物点形成的似然场的物理尺寸也变得更大。
为了允许由于位置相同但是朝向不同，导致扫描数据与地图存在较大差异，在多分辨率配准中，只要配准点数超过所有扫描数据的一定比例，就认为地图匹配上了。这个比例是回环检测中的一个关键阈值，体现了回环检测的敏感程度，比例越高，越准确严格。
6.5.2基于子地图的回环修正
如果回环检测成功得到了当前帧与历史子地图之间的配准关系则会启动一次回环修正。考虑当前帧与某个历史子地图之间的关系。我们通过多分辨率匹配计算当前帧在历史子地图中的位姿。记历史子地图,本身的位姿为
,，当前帧自身的位姿为
，当前帧所在的子地图
,的位姿为
。那么多分辨率匹配实际计算的应该是相对位姿
。于是，可以把这个结果转换为历史子地图与当前子地图的位姿变换：
在回环优化中，以每个子地图的位姿为优化变量，构建一个位姿图进行优化。该位姿图的观测来源于两种：一种是相邻位姿图之间的相对位姿观测，另一种是回环检测计算出来的历史地图与子地图之间的相对位姿关系。仍要考虑子地图与子地图
之间的相对对位姿，假设回环检测计算出的相对位姿观测量为
,那么他们之间的残差为：
回环检测实现代码如下：头文件
/**
* @class LoopClosing
* @brief 基于子地图的回环检测与位姿图优化模块
* @details 核心功能：
* 1. 管理子地图的创建、完成状态与似然场映射
* 2. 检测当前帧与历史子地图的回环候选
* 3. 通过最大似然场（MR）匹配验证回环有效性
* 4. 构建子地图间的位姿图并优化，更新全局位姿
*/
class LoopClosing {
public:
/**
* @struct LoopConstraints
* @brief 子地图间的回环约束结构体
* @details 存储两个子地图的相对位姿约束，以及约束的有效性标记
*/
struct LoopConstraints {
size_t id_submap1_ = 0; ///< 源子地图ID（约束起点）
size_t id_submap2_ = 0; ///< 目标子地图ID（约束终点）
SE2 T12_; ///< 子地图1到子地图2的相对位姿 T_{12} = T_1⁻¹ * T_2
bool valid_ = true; ///< 约束有效性标记（true=内点/有效，false=外点/无效）
/**
* @brief 回环约束构造函数
* @param id1 源子地图ID
* @param id2 目标子地图ID
* @param T12 子地图1到子地图2的相对位姿
*/
LoopConstraints(size_t id1, size_t id2, const SE2& T12)
: id_submap1_(id1), id_submap2_(id2), T12_(T12) {}
};
/**
* @brief 构造函数
* @details 初始化调试文件输出流，打开回环检测日志文件
*/
LoopClosing() { debug_fout_.open("./data/ch6/loops.txt"); }
/**
* @brief 析构函数
* @details 关闭调试文件输出流，释放资源
*/
~LoopClosing() {
if (debug_fout_.is_open()) {
debug_fout_.close();
}
}
/**
* @brief 添加新创建的子地图（未完成状态）
* @param submap 待添加的子地图共享指针
* @note 该子地图可能仍在构建中（新增关键帧），需后续调用AddFinishedSubmap标记完成
*/
void AddNewSubmap(std::shared_ptr<Submap> submap);
/**
* @brief 添加已完成的子地图（构建似然场用于回环匹配）
* @param submap 已完成的子地图共享指针
* @note 必须在AddNewSubmap之后调用，完成子地图到似然场的映射初始化
*/
void AddFinishedSubmap(std::shared_ptr<Submap> submap);
/**
* @brief 处理新帧，执行完整回环检测流程
* @param frame 新输入的帧（关键帧）共享指针
* @details 流程：
* 1. 检测当前帧的回环候选子地图
* 2. 与候选子地图进行MR匹配验证回环
* 3. 若检测到新回环，执行位姿图优化
* 4. 更新帧和子地图的全局位姿
*/
void AddNewFrame(std::shared_ptr<Frame> frame);
/**
* @brief 获取所有有效/无效的回环约束
* @return 以<子地图ID对, 回环约束>为键值对的映射表
*/
std::map<std::pair<size_t, size_t>, LoopConstraints> GetLoops() const { return loop_constraints_; }
/**
* @brief 检查是否检测到新的有效回环
* @return true=有新回环，false=无新回环
*/
bool HasNewLoops() const { return has_new_loops_; }
private:
/**
* @brief 检测当前帧的回环候选子地图
* @return true=找到候选子地图，false=无候选
* @details 筛选规则：
* 1. 跳过最近submap_gap_个子地图（避免短期重复匹配）
* 2. 跳过已有有效回环约束的子地图
* 3. 筛选与当前帧中心距离小于candidate_distance_th_的子地图
*/
bool DetectLoopCandidates();
/**
* @brief 将当前帧与候选历史子地图进行MR匹配验证回环
* @details 核心逻辑：
* 1. 从似然场中获取候选子地图的匹配器
* 2. 计算当前帧在候选子地图坐标系下的初始位姿
* 3. 执行G2O对齐验证回环，记录有效约束
* 4. 输出调试信息并可视化匹配结果
*/
void MatchInHistorySubmaps();
/**
* @brief 执行子地图间的位姿图优化
* @details 基于g2o实现SE2位姿图优化：
* 1. 构建子地图顶点（SE2位姿）
* 2. 添加连续子地图的相邻约束（强约束）
* 3. 添加回环约束（带Cauchy鲁棒核，处理外点）
* 4. 两轮优化（鲁棒核筛选内点 + 精修）
* 5. 更新子地图位姿并清理无效约束
*/
void Optimize();
// ---------------- 状态变量 ----------------
std::shared_ptr<Frame> current_frame_ = nullptr; ///< 当前处理的帧（关键帧）
size_t last_submap_id_ = 0; ///< 最新添加的子地图ID（用于筛选候选）
// ---------------- 数据容器 ----------------
std::map<size_t, std::shared_ptr<Submap>> submaps_; ///< 所有子地图的映射表（ID -> 子地图）
std::map<std::shared_ptr<Submap>, std::shared_ptr<MRLikelihoodField>> submap_to_field_; ///< 子地图到似然场的映射（用于匹配）
std::vector<size_t> current_candidates_; ///< 当前帧的回环候选子地图ID列表
std::map<std::pair<size_t, size_t>, LoopConstraints> loop_constraints_; ///< 所有回环约束（<子地图1ID, 子地图2ID> -> 约束）
bool has_new_loops_ = false; ///< 标记是否检测到新的有效回环（触发优化）
// ---------------- 调试工具 ----------------
std::ofstream debug_fout_; ///< 回环检测调试日志输出流（记录帧ID、子地图ID、位姿等）
// ---------------- 配置参数（静态常量） ----------------
inline static constexpr float candidate_distance_th_ = 15.0; ///< 候选筛选距离阈值（米）：帧与子地图中心的最大距离
inline static constexpr int submap_gap_ = 1; ///< 子地图间隔阈值：跳过最近N个子地图（避免短期回环）
inline static constexpr double loop_rk_delta_ = 1.0; ///< 回环约束鲁棒核阈值（Cauchy核）：用于筛选内点/外点
};.c文件
/// 回环检测与位姿图优化类
/// 负责子地图管理、回环候选检测、回环匹配验证、位姿图优化
class LoopClosing {
public:
/// 回环约束结构体
struct LoopConstraints {
size_t submap_id1_; // 源子地图ID
size_t submap_id2_; // 目标子地图ID
SE2 T12_; // 子地图1到子地图2的相对位姿 T_{12} = T_1^{-1}*T_2
bool valid_ = true; // 约束是否有效（内点）
LoopConstraints(size_t id1, size_t id2, const SE2& T12)
: submap_id1_(id1), submap_id2_(id2), T12_(T12) {}
};
/// 构造函数（补充默认构造，初始化参数）
LoopClosing() : submap_gap_(5), candidate_distance_th_(10.0), loop_rk_delta_(1.0) {
debug_fout_.open("./loop_debug.txt"); // 调试文件输出流
}
/// 析构函数
~LoopClosing() {
if (debug_fout_.is_open()) {
debug_fout_.close();
}
}
/**
* @brief 将完成的子地图转换为匹配用的似然场并存储映射关系
* @param submap 已完成的子地图共享指针
* @note 似然场（MRLikelihoodField）用于激光扫描与子地图的匹配
*/
void AddFinishedSubmap(std::shared_ptr<Submap> submap) {
// 创建似然场对象
auto mr_field = std::make_shared<MRLikelihoodField>();
// 设置似然场的位姿为子地图位姿
mr_field->SetPose(submap->GetPose());
// 从子地图的占据栅格地图初始化似然场的场图像
mr_field->SetFieldImageFromOccuMap(submap->GetOccuMap().GetOccupancyGrid());
// 建立子地图到似然场的映射
submap_to_field_.emplace(submap, mr_field);
}
/**
* @brief 添加新的子地图到管理容器
* @param submap 新创建的子地图共享指针
* @note 维护子地图ID的递增管理，更新最新子地图ID
*/
void AddNewSubmap(std::shared_ptr<Submap> submap) {
// 将子地图存入ID到子地图的映射容器
submaps_.emplace(submap->GetId(), submap);
// 更新最新子地图ID
last_submap_id_ = submap->GetId();
}
/**
* @brief 处理新帧，触发完整的回环检测流程
* @param frame 新的关键帧共享指针
* @note 流程：检测回环候选 -> 与候选子地图匹配 -> 有新回环则优化位姿图
*/
void AddNewFrame(std::shared_ptr<Frame> frame) {
// 保存当前帧
current_frame_ = frame;
// 检测回环候选子地图，无候选则直接返回
if (!DetectLoopCandidates()) {
return;
}
// 与候选历史子地图进行回环匹配
MatchInHistorySubmaps();
// 如果检测到新的有效回环，执行位姿图优化
if (has_new_loops_) {
Optimize();
}
}
/**
* @brief 检测回环候选子地图
* @return 是否找到有效候选子地图
* @note 筛选规则：
* 1. 排除最近的submap_gap_个子地图（避免短期回环）
* 2. 排除已有有效回环约束的子地图
* 3. 按当前帧与子地图中心的距离筛选（小于阈值为候选）
*/
bool DetectLoopCandidates() {
// 初始化回环标记为无新回环
has_new_loops_ = false;
// 最新子地图ID过小（还未生成足够子地图），直接返回无候选
if (last_submap_id_ < submap_gap_) {
return false;
}
// 清空上一轮的候选列表
current_candidates_.clear();
// 遍历所有已存储的子地图
for (auto& sp : submaps_) {
size_t submap_id = sp.first;
std::shared_ptr<Submap> submap = sp.second;
// 1. 过滤最近的submap_gap_个子地图（避免检测近期子地图）
if ((last_submap_id_ - submap_id) <= submap_gap_) {
continue;
}
// 2. 过滤已有有效回环约束的子地图（避免重复处理）
std::pair<size_t, size_t> constraint_key(submap_id, last_submap_id_);
auto hist_iter = loop_constraints_.find(constraint_key);
if (hist_iter != loop_constraints_.end() && hist_iter->second.valid_) {
continue;
}
// 3. 计算当前帧与子地图中心的欧氏距离，筛选近距离候选
Vec2d submap_center = submap->GetPose().translation(); // 子地图中心坐标
Vec2d frame_pos = current_frame_->pose_.translation(); // 当前帧位姿坐标
double distance = (submap_center - frame_pos).norm(); // 欧式距离
// 距离小于阈值则加入候选列表
if (distance < candidate_distance_th_) {
LOG(INFO) << "taking " << current_frame_->keyframe_id_ << " with " << submap_id
<< ", last submap id: " << last_submap_id_;
current_candidates_.emplace_back(submap_id);
}
}
// 返回是否有有效候选
return !current_candidates_.empty();
}
/**
* @brief 与候选历史子地图进行回环匹配验证
* @note 核心流程：
* 1. 从似然场中获取候选子地图的匹配器
* 2. 初始化激光扫描与子地图的初始位姿
* 3. 通过G2O对齐验证回环有效性
* 4. 记录有效回环约束，可视化匹配结果
*/
void MatchInHistorySubmaps() {
// 遍历所有候选子地图ID
for (const size_t& candidate_id : current_candidates_) {
// 获取候选子地图对应的似然场匹配器
auto mr_matcher = submap_to_field_.at(submaps_[candidate_id]);
// 设置匹配器的源激光扫描数据（当前帧的扫描）
mr_matcher->SetSourceScan(current_frame_->scan_);
// 获取候选子地图对象
auto candidate_submap = submaps_[candidate_id];
// 计算当前帧在候选子地图坐标系下的初始位姿：T_{S1_C} = T_{S1}^{-1} * T_C
// S1: 候选子地图坐标系, C: 当前帧坐标系
SE2 pose_in_target_submap = candidate_submap->GetPose().inverse() * current_frame_->pose_;
// 通过G2O进行似然场对齐，验证回环是否有效
if (mr_matcher->AlignG2O(pose_in_target_submap)) {
// 计算两个子地图间的相对位姿约束：T_{S1_S2} = T_{S1_C} * T_C^{-1} * T_{S2}
// S1: 候选子地图, S2: 最新子地图, T_{S1_S2} = S1到S2的相对位姿
SE2 T_submap1_submap2 =
pose_in_target_submap * current_frame_->pose_.inverse() * submaps_[last_submap_id_]->GetPose();
// 记录回环约束（候选子地图ID -> 最新子地图ID）
loop_constraints_.emplace(
std::pair<size_t, size_t>(candidate_id, last_submap_id_),
LoopConstraints(candidate_id, last_submap_id_, T_submap1_submap2)
);
LOG(INFO) << "adding loop from submap " << candidate_id << " to " << last_submap_id_;
/// 可视化回环匹配结果
// 获取子地图的占据栅格图像（黑白）
auto occu_image = candidate_submap->GetOccuMap().GetOccupancyGridBlackWhite();
// 在子地图图像上绘制当前帧的激光扫描（红色）
Visualize2DScan(current_frame_->scan_, pose_in_target_submap, occu_image, Vec3b(0, 0, 255), 1000, 20.0, SE2());
// 添加文字标注：子地图ID
cv::putText(occu_image, "loop submap " + std::to_string(candidate_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(candidate_submap->NumFrames()),
cv::Point2f(20, 50), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
// 显示可视化结果
cv::imshow("loop closure", occu_image);
cv::waitKey(10); // 短暂等待以更新窗口
// 标记存在新的有效回环
has_new_loops_ = true;
}
// 输出调试信息：帧ID、候选子地图ID、子地图位姿（x,y,yaw）
debug_fout_ << current_frame_->id_ << " " << candidate_id
<< " " << candidate_submap->GetPose().translation().x()
<< " " << candidate_submap->GetPose().translation().y()
<< " " << candidate_submap->GetPose().so2().log() << std::endl;
}
// 清空候选列表
current_candidates_.clear();
}
/**
* @brief 位姿图优化（核心优化函数）
* @note 基于g2o实现SE2位姿图优化，包含两类约束：
* 1. 连续子地图的相邻约束（强约束）
* 2. 回环约束（带鲁棒核的弱约束）
* 优化流程：
* 1. 构建优化器，添加子地图顶点
* 2. 添加连续约束和回环约束
* 3. 两轮优化（鲁棒核筛选内点 + 精修）
* 4. 更新子地图位姿，清理无效约束
*/
void Optimize() {
// ========== 1. 初始化g2o优化器 ==========
// 定义块求解器（SE2位姿：3维(xyyaw)，观测：1维-实际为SE2相对位姿）
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
// 定义线性求解器（Cholmod稀疏矩阵求解器）
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
// 创建Levenberg-Marquardt优化算法
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true); // 输出优化日志
// ========== 2. 添加子地图顶点 ==========
for (auto& sp : submaps_) {
size_t submap_id = sp.first;
std::shared_ptr<Submap> submap = sp.second;
// 创建SE2顶点，ID为子地图ID
auto* vertex = new VertexSE2();
vertex->setId(submap_id);
// 设置顶点初始估计为子地图当前位姿
vertex->setEstimate(submap->GetPose());
// 添加顶点到优化器
optimizer.addVertex(vertex);
}
// ========== 3. 添加连续子地图约束（相邻子地图的相对位姿约束） ==========
for (int i = 0; i < last_submap_id_; ++i) {
auto first_submap = submaps_[i];
auto next_submap = submaps_[i + 1];
// 创建SE2边（约束）
EdgeSE2* edge = new EdgeSE2();
// 设置边的关联顶点（i -> i+1）
edge->setVertex(0, optimizer.vertex(i));
edge->setVertex(1, optimizer.vertex(i + 1));
// 设置测量值：相邻子地图的相对位姿 T_{i, i+1} = T_i^{-1} * T_{i+1}
edge->setMeasurement(first_submap->GetPose().inverse() * next_submap->GetPose());
// 设置信息矩阵（权重）：单位矩阵*1e4（强约束）
edge->setInformation(Mat3d::Identity() * 1e4);
// 添加边到优化器
optimizer.addEdge(edge);
}
// ========== 4. 添加回环约束 ==========
std::map<std::pair<size_t, size_t>, EdgeSE2*> loop_edges; // 回环边映射
for (auto& constraint : loop_constraints_) {
// 跳过无效约束
if (!constraint.second.valid_) {
continue;
}
size_t submap1_id = constraint.first.first;
size_t submap2_id = constraint.first.second;
auto submap1 = submaps_[submap1_id];
auto submap2 = submaps_[submap2_id];
// 创建回环约束边
EdgeSE2* edge = new EdgeSE2();
edge->setVertex(0, optimizer.vertex(submap1_id));
edge->setVertex(1, optimizer.vertex(submap2_id));
// 设置测量值：回环检测得到的子地图相对位姿
edge->setMeasurement(constraint.second.T12_);
// 设置信息矩阵（权重）：单位矩阵（弱约束）
edge->setInformation(Mat3d::Identity());
// 添加Cauchy鲁棒核（处理外点）
auto robust_kernel = new g2o::RobustKernelCauchy;
robust_kernel->setDelta(loop_rk_delta_); // 鲁棒核阈值
edge->setRobustKernel(robust_kernel);
// 添加边到优化器并记录
optimizer.addEdge(edge);
loop_edges.emplace(constraint.first, edge);
}
// ========== 5. 执行优化 ==========
optimizer.initializeOptimization();
optimizer.optimize(10); // 第一轮优化（鲁棒核筛选内点）
// ========== 6. 验证回环约束，标记内点/外点 ==========
int inlier_count = 0;
for (auto& edge_pair : loop_edges) {
EdgeSE2* edge = edge_pair.second;
double chi2 = edge->chi2(); // 边的卡方值（残差平方和）
// 卡方值小于阈值为内点
if (chi2 < loop_rk_delta_) {
LOG(INFO) << "loop from " << edge_pair.first.first << " to " << edge_pair.first.second
<< " is correct, chi2: " << chi2;
edge->setRobustKernel(nullptr); // 移除鲁棒核
loop_constraints_.at(edge_pair.first).valid_ = true;
inlier_count++;
} else {
// 外点：设置边为层级1（优化时忽略）
edge->setLevel(1);
LOG(INFO) << "loop from " << edge_pair.first.first << " to " << edge_pair.first.second
<< " is invalid, chi2: " << chi2;
loop_constraints_.at(edge_pair.first).valid_ = false;
}
}
// 第二轮优化（仅优化内点，精修位姿）
optimizer.optimize(5);
// ========== 7. 更新子地图位姿 ==========
for (auto& sp : submaps_) {
VertexSE2* vertex = static_cast<VertexSE2*>(optimizer.vertex(sp.first));
// 更新子地图的位姿为优化后结果
sp.second->SetPose(vertex->estimate());
// 更新子地图所属所有帧的世界位姿
sp.second->UpdateFramePoseWorld();
}
LOG(INFO) << "loop inliers: " << inlier_count << "/" << loop_constraints_.size();
// ========== 8. 清理无效回环约束 ==========
for (auto iter = loop_constraints_.begin(); iter != loop_constraints_.end();) {
if (!iter->second.valid_) {
iter = loop_constraints_.erase(iter); // 擦除外点约束
} else {
iter++;
}
}
}
private:
// 配置参数
size_t submap_gap_; // 回环检测跳过的最近子地图数量
double candidate_distance_th_; // 回环候选距离阈值（米）
double loop_rk_delta_; // 回环约束鲁棒核阈值
// 状态变量
size_t last_submap_id_ = 0; // 最新子地图ID
bool has_new_loops_ = false; // 是否检测到新回环
std::shared_ptr<Frame> current_frame_; // 当前处理的帧
std::vector<size_t> current_candidates_; // 当前回环候选子地图ID列表
// 数据容器
std::map<size_t, std::shared_ptr<Submap>> submaps_; // 子地图ID到子地图的映射
std::map<std::shared_ptr<Submap>, std::shared_ptr<MRLikelihoodField>> submap_to_field_; // 子地图到似然场的映射
std::map<std::pair<size_t, size_t>, LoopConstraints> loop_constraints_; // 回环约束映射（子地图ID对 -> 约束）
// 调试输出
std::ofstream debug_fout_; // 调试信息输出文件流
// 可视化辅助函数（声明）
void Visualize2DScan(const std::vector<Vec2d>& scan, const SE2& pose, cv::Mat& img,
const cv::Vec3b& color, int img_size, double scale, const SE2& offset) {
// 实现激光扫描可视化逻辑（此处为占位，需根据实际需求补充）
}
};该部分代码在每个子地图完成时建立它的多分辨率似然场,然后将最近的 Scan 与建完的子地图进行匹配。如果多分辨率匹配成功,就调用 Optimize函数进行一次回环修正。修正的结果将直接影响每个子地图的位姿。
回环修正之后的全局地图以及未经处理的全局地图对比。
ps：第6章完啦，开启第七章去
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
13
0- 0
已为社区贡献5条内容
所有评论(0)