Cartographer 前端 Local SLAM 详解（２）

Cartographer 前端 Local SLAM 详解

1. 前端概述

1.1 Local SLAM 的职责

核心任务:

• 实时位姿跟踪: 在 10-100Hz 的频率下估计机器人位姿
• 局部地图构建: 维护短期 Submaps (通常包含 45-90 次扫描)
• 数据预处理: 滤波、降采样、异常值剔除

与后端的关系:

前端输出 → 后端输入
├─ Node (位姿节点)
├─ Submap (子图)
└─ Constraint (局部约束)

性能指标:

• 延迟: < 100ms (从接收点云到输出位姿)
• 精度: 局部漂移 < 1% 路径长度
• 鲁棒性: 处理传感器噪声、动态物体

1.2 数据流管线

RangeData (原始点云)
↓
[1] 预处理阶段
├─ Voxel Filter (体素下采样)
├─ Adaptive Voxel Filter (自适应采样)
└─ Range Filter (距离裁剪)
↓
[2] 位姿预测阶段
├─ Pose Extrapolator (融合 IMU/Odometry)
└─ 输出初始位姿估计
↓
[3] 扫描匹配阶段
├─ CSM (Correlative Scan Matcher)
│ └─ 暴力搜索提供粗匹配
├─ Ceres Scan Matcher
│ └─ 非线性优化精修位姿
└─ 输出优化后位姿
↓
[4] Submap 更新阶段
├─ 射线投射 (Ray Tracing)
├─ 概率更新 (Bayesian Update)
└─ Submap 状态管理
↓
[5] 运动过滤阶段
├─ Motion Filter (过滤微小运动)
└─ 决定是否插入新 Node
↓
InsertionResult (输出给后端)

2. 数据预处理

2.1 Voxel Filter (体素滤波器)

目的: 减少点云数量，降低计算负载。

算法原理:

// sensor/voxel_filter.cc
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
// 1. 创建体素网格 (3D 哈希表)
absl::flat_hash_map<VoxelKey, Eigen::Vector3f> voxel_map;

for (const RangefinderPoint& point : point_cloud) {
// 2. 计算点所属的体素索引
// key = floor(point / voxel_size)
const VoxelKey key = ComputeVoxelKey(point.position, resolution_);

// 3. 每个体素只保留一个点 (覆盖策略)
// 可选策略: 中心点、质心、最近点
voxel_map[key] = point.position;
}

// 4. 提取滤波后的点云
PointCloud filtered;
filtered.reserve(voxel_map.size());
for (const auto& [key, point] : voxel_map) {
filtered.push_back({point});
}

return filtered;
}

// 体素键计算 (整数网格坐标)
VoxelKey ComputeVoxelKey(const Eigen::Vector3f& point, float resolution) {
return VoxelKey{
.x = static_cast<int>(std::floor(point.x() / resolution)),
.y = static_cast<int>(std::floor(point.y() / resolution)),
.z = static_cast<int>(std::floor(point.z() / resolution))
};
}

参数配置:

— trajectory_builder_2d.lua
TRAJECTORY_BUILDER_2D = {
voxel_filter_size = 0.025, — 2.5cm 体素尺寸
— 对于 40m 范围内的点云:
— 原始: ~10000 点
— 滤波后: ~2000 点 (减少 80%)
}

性能分析:

• 时间复杂度: $O(N)$ (哈希表插入)
• 空间复杂度: $O(M)$ (M = 非空体素数)
• 数据压缩率: 典型 5:1 (取决于点云密度)

2.2 Adaptive Voxel Filter (自适应体素滤波器)

问题: 固定体素尺寸可能导致:

• 远距离点云过度稀疏 → 匹配失败
• 近距离点云仍过密 → 计算浪费

解决方案: 动态调整体素尺寸以保证最小点数。

算法实现:

// sensor/internal/voxel_filter.cc
PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) {
float current_resolution = max_length_; // 初始尺寸 (最粗)

while (true) {
// 1. 使用当前分辨率滤波
PointCloud filtered = VoxelFilter::Filter(
point_cloud, current_resolution);

// 2. 检查点数是否满足要求
if (filtered.size() >= min_num_points_) {
return filtered; // 满足条件，返回结果
}

// 3. 点数不足，减小体素尺寸 (增加密度)
current_resolution *= 0.5f;

// 4. 防止无限循环 (最小尺寸限制)
if (current_resolution < max_length_ * 1e-6f) {
LOG(WARNING) << "Cannot achieve min_num_points: "
<< filtered.size() << " < " << min_num_points_;
return filtered;
}
}
}

参数配置:

TRAJECTORY_BUILDER_2D = {
adaptive_voxel_filter = {
max_length = 0.5, — 初始体素尺寸 (最粗)
min_num_points = 200, — 最少保留点数
max_range = 50.0, — 最大有效距离
},
}

应用场景:

2.3 Range Filter (距离过滤器)

目的: 剔除无效测量点。

过滤规则:

// sensor/range_data.cc
RangeData CropRangeData(const RangeData& range_data,
const float min_range,
const float max_range) {
RangeData cropped;

for (const RangefinderPoint& point : range_data.returns) {
const float range = point.position.norm();

// 1. 过滤太近的点 (传感器盲区)
if (range < min_range) continue;

// 2. 过滤太远的点 (噪声大、精度低)
if (range > max_range) continue;

// 3. 过滤 NaN/Inf 值
if (!std::isfinite(range)) continue;

cropped.returns.push_back(point);
}

return cropped;
}

典型参数:

TRAJECTORY_BUILDER_2D = {
min_range = 0.2, — 20cm 以内为盲区
max_range = 30.0, — 30m 以外丢弃
— 原因: LiDAR 精度随距离下降
— σ_range ≈ 0.01m + 0.001 × distance
}

3. 位姿预测 (Pose Extrapolator)

3.1 多传感器融合

目的: 在扫描匹配前提供初始位姿估计。

数据来源:

• 提供: 角速度 $\omega$, 线加速度 $a$
• 优势: 高频 (200-1000Hz)，短期精度高
• 劣势: 存在零偏漂移

• 提供: 位移增量 $\Delta x,\Delta y,\Delta \theta$
• 优势: 长期稳定性好
• 劣势: 打滑误差、机械误差

融合策略:

// mapping/internal/pose_extrapolator.cc
class PoseExtrapolator {
public:
// 添加 IMU 数据
void AddImuData(const sensor::ImuData& imu_data) {
imu_tracker_->AddImuData(imu_data);

// 更新重力方向估计 (用于水平校正)
gravity_direction_ = imu_tracker_->orientation() *
Eigen::Vector3d::UnitZ();
}

// 添加里程计数据
void AddOdometryData(const sensor::OdometryData& odometry_data) {
// 计算里程计速度 (差分)
if (timed_pose_queue_.empty()) {
odometry_pose_ = odometry_data.pose;
return;
}

const auto delta_pose = odometry_pose_.inverse() * odometry_data.pose;
const double delta_time =
common::ToSeconds(odometry_data.time – last_odometry_time_);

linear_velocity_from_odometry_ =
delta_pose.translation() / delta_time;
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
delta_pose.rotation()) / delta_time;
}

// 预测当前时刻位姿
transform::Rigid3d ExtrapolatePose(const common::Time time) {
const auto last_pose = timed_pose_queue_.back();
const double extrapolation_duration =
common::ToSeconds(time – last_pose.time);

// 1. IMU 提供旋转增量
const Eigen::Quaterniond rotation =
imu_tracker_->orientation() *
last_pose.pose.rotation().inverse() *
last_pose.pose.rotation();

// 2. 里程计提供平移增量
const Eigen::Vector3d translation =
last_pose.pose.translation() +
linear_velocity_from_odometry_ * extrapolation_duration;

return transform::Rigid3d(translation, rotation);
}

private:
std::unique_ptr<ImuTracker> imu_tracker_;
std::deque<TimedPose> timed_pose_queue_; // 历史位姿队列
Eigen::Vector3d linear_velocity_from_odometry_;
Eigen::Vector3d angular_velocity_from_odometry_;
};

3.2 IMU 数据处理

重力对齐 (Gravity Alignment):

// mapping/internal/imu_tracker.cc
void ImuTracker::AddImuData(const sensor::ImuData& imu_data) {
const double delta_t =
common::ToSeconds(imu_data.time – time_);

// 1. 更新角速度积分 (旋转估计)
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
imu_data.angular_velocity * delta_t);
orientation_ = (orientation_ * rotation).normalized();

// 2. 重力方向滤波 (低通滤波消除高频噪声)
// α ∈ [0, 1]: 滤波系数，越小越平滑
const double alpha = 1.0 – std::exp(–delta_t / gravity_time_constant_);

Eigen::Vector3d gravity_in_tracking_frame =
orientation_.conjugate() * imu_data.linear_acceleration;

gravity_direction_ =
(1.0 – alpha) * gravity_direction_ +
alpha * gravity_in_tracking_frame.normalized();

// 3. 纠正旋转 (使 Z 轴对齐重力方向)
const Eigen::Quaterniond gravity_alignment =
Eigen::Quaterniond::FromTwoVectors(
orientation_ * Eigen::Vector3d::UnitZ(),
gravity_direction_);

orientation_ = gravity_alignment * orientation_;
}

数学原理:

$${\mathbf{q}}_{t+1}={\mathbf{q}}_t\cdot \exp \left(\frac{\Delta t}{2}{\mathbf{\omega }}_t\right)$$

其中 ${\mathbf{\omega }}_t$ 为角速度，$\exp (\cdot )$ 为四元数指数映射。

4. 扫描匹配 (Scan Matching)

4.1 CSM (Correlative Scan Matcher) – 暴力搜索

目标: 在搜索窗口内找到最佳匹配位姿。

算法流程:

// mapping/scan_matching/real_time_correlative_scan_matcher_2d.cc
bool RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* pose_estimate) {

// 1. 定义搜索参数
const SearchParameters search_parameters(
linear_search_window_, // ±0.1m
angular_search_window_, // ±0.2rad (约 ±11°)
point_cloud,
grid.limits().resolution()
);

// 2. 生成候选位姿 (离散化搜索空间)
const std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);

// 候选数量计算:
// N = (2 * linear_window / resolution + 1)^2 *
// (2 * angular_window / angular_step + 1)
// 典型: (21)^2 * (41) ≈ 18000 个候选

// 3. 并行评分 (OpenMP 加速)
std::vector<Candidate2D> scored_candidates =
ComputeCandidatesScores(candidates, point_cloud, grid);

// 4. 选择最佳候选
const Candidate2D& best_candidate =
*std::max_element(
scored_candidates.begin(),
scored_candidates.end(),
[](const Candidate2D& a, const Candidate2D& b) {
return a.score < b.score;
});

// 5. 阈值检查
if (best_candidate.score < min_score_) {
return false; // 匹配失败
}

*pose_estimate = initial_pose_estimate * best_candidate.pose;
return true;
}

评分函数:

float ComputeCandidateScore(
const Candidate2D& candidate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid) {

float score = 0.0f;
int num_valid_points = 0;

for (const RangefinderPoint& point : point_cloud) {
// 1. 变换点云到候选位姿
const Eigen::Vector2f transformed_point =
candidate.pose.cast<float>() * point.position.head<2>();

// 2. 查询占用概率
const float probability =
grid.GetProbability(grid.limits().GetCellIndex(transformed_point));

// 3. 累加分数 (占用区域分数高)
if (probability != kUnknownProbability) {
score += probability;
++num_valid_points;
}
}

// 4. 归一化
return num_valid_points > 0 ? score / num_valid_points : 0.0f;
}

性能优化:

// 预计算点云旋转 (避免重复三角函数运算)
class RotatingRangeScanMatcher {
void PrecomputeRotatedPointClouds(
const sensor::PointCloud& point_cloud,
const std::vector<float>& angles) {

rotated_scans_.clear();
rotated_scans_.reserve(angles.size());

for (const float angle : angles) {
const Eigen::Rotation2Df rotation(angle);

sensor::PointCloud rotated;
rotated.reserve(point_cloud.size());

for (const auto& point : point_cloud) {
rotated.push_back({rotation * point.position.head<2>()});
}

rotated_scans_.push_back(std::move(rotated));
}
}

std::vector<sensor::PointCloud> rotated_scans_;
};

4.2 Ceres Scan Matcher – 非线性优化

目标: 在 CSM 提供的初值基础上，精确求解位姿。

Cost Function 定义:

// mapping/scan_matching/ceres_scan_matcher_2d.cc
class OccupiedSpaceCostFunction2D {
public:
OccupiedSpaceCostFunction2D(
const double scaling_factor,
const sensor::PointCloud& point_cloud,
const Grid2D& grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
grid_(grid) {}

template <typename T>
bool operator()(const T* const pose, T* residual) const {
// pose = [x, y, theta]
const Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
const Eigen::Rotation2D<T> rotation(pose[2]);

// 遍历所有点计算残差
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// 1. 变换点云
const Eigen::Matrix<T, 2, 1> point(
T(point_cloud_[i].position.x()),
T(point_cloud_[i].position.y())
);
const Eigen::Matrix<T, 2, 1> world_point =
rotation * point + translation;

// 2. 双线性插值查询占用概率
const T probability =
InterpolateProbability(world_point, grid_);

// 3. 残差 = 1 – 占用概率 (被占用区域残差小)
residual[i] = scaling_factor_ * (T(1.0) – probability);
}

return true;
}

private:
// 双线性插值 (平滑梯度，有利于优化)
template <typename T>
static T InterpolateProbability(
const Eigen::Matrix<T, 2, 1>& point,
const Grid2D& grid) {

const MapLimits& limits = grid.limits();
const T x_index = (point.x() – limits.min().x()) / limits.resolution();
const T y_index = (point.y() – limits.min().y()) / limits.resolution();

// 获取四个邻近栅格
const int x0 = static_cast<int>(std::floor(x_index));
const int y0 = static_cast<int>(std::floor(y_index));

const T p00 = T(grid.GetProbability(Eigen::Array2i(x0, y0)));
const T p01 = T(grid.GetProbability(Eigen::Array2i(x0, y0 + 1)));
const T p10 = T(grid.GetProbability(Eigen::Array2i(x0 + 1, y0)));
const T p11 = T(grid.GetProbability(Eigen::Array2i(x0 + 1, y0 + 1)));

// 双线性插值
const T wx = x_index – T(x0);
const T wy = y_index – T(y0);

return (T(1.0) – wx) * (T(1.0) – wy) * p00 +
(T(1.0) – wx) * wy * p01 +
wx * (T(1.0) – wy) * p10 +
wx * wy * p11;
}

const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const Grid2D& grid_;
};

正则化项 (防止过拟合):

// 平移正则化
class TranslationDeltaCostFunction2D {
public:
TranslationDeltaCostFunction2D(
const double scaling_factor,
const transform::Rigid2d& initial_pose)
: scaling_factor_(scaling_factor),
x_(initial_pose.translation().x()),
y_(initial_pose.translation().y()) {}

template <typename T>
bool operator()(const T* const pose, T* residual) const {
// 惩罚远离初值的平移
residual[0] = scaling_factor_ * (pose[0] – T(x_));
residual[1] = scaling_factor_ * (pose[1] – T(y_));
return true;
}

private:
const double scaling_factor_;
const double x_, y_;
};

// 旋转正则化
class RotationDeltaCostFunction2D {
public:
RotationDeltaCostFunction2D(
const double scaling_factor,
const double initial_angle)
: scaling_factor_(scaling_factor),
angle_(initial_angle) {}

template <typename T>
bool operator()(const T* const pose, T* residual) const {
// 角度归一化到 [-π, π]
residual[0] = scaling_factor_ *
NormalizeAngle(pose[2] – T(angle_));
return true;
}

private:
const double scaling_factor_;
const double angle_;
};

构建优化问题:

void CeresScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* pose_estimate,
ceres::Solver::Summary* summary) {

// 1. 初始化优化变量
double pose[3] = {
initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()
};

// 2. 构建问题
ceres::Problem problem;

// 2.1 添加占用空间约束
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight(),
point_cloud,
grid),
nullptr, // 损失函数 (或使用 HuberLoss)
pose
);

// 2.2 添加平移正则化
problem.AddResidualBlock(
CreateTranslationDeltaCostFunction2D(
options_.translation_weight(),
initial_pose_estimate),
nullptr,
pose
);

// 2.3 添加旋转正则化
problem.AddResidualBlock(
CreateRotationDeltaCostFunction2D(
options_.rotation_weight(),
initial_pose_estimate.rotation().angle()),
nullptr,
pose
);

// 3. 配置求解器
ceres::Solver::Options solver_options;
solver_options.max_num_iterations = options_.num_iterations();
solver_options.num_threads = options_.num_threads();
solver_options.linear_solver_type = ceres::DENSE_QR;

// 4. 求解
ceres::Solve(solver_options, &problem, summary);

// 5. 提取结果
*pose_estimate = transform::Rigid2d(
{pose[0], pose[1]}, pose[2]);
}

参数配置:

TRAJECTORY_BUILDER_2D.ceres_scan_matcher = {
occupied_space_weight = 1.0, — 占用空间约束权重
translation_weight = 10.0, — 平移正则化权重
rotation_weight = 40.0, — 旋转正则化权重

num_iterations = 10, — 最大迭代次数
num_threads = 1, — 线程数
}

5. Submap 插入

5.1 射线投射 (Ray Tracing)

目的: 更新从传感器到击中点路径上的所有栅格。

Bresenham 算法实现:

// mapping/2d/ray_to_pixel_mask.cc
std::vector<Eigen::Array2i> RayToPixelMask(
const Eigen::Array2i& scaled_begin,
const Eigen::Array2i& scaled_end,
int subpixel_scale) {

std::vector<Eigen::Array2i> pixel_mask;

// Bresenham 直线算法
int dx = std::abs(scaled_end.x() – scaled_begin.x());
int dy = std::abs(scaled_end.y() – scaled_begin.y());

int sx = scaled_begin.x() < scaled_end.x() ? 1 : –1;
int sy = scaled_begin.y() < scaled_end.y() ? 1 : –1;

int err = dx – dy;
Eigen::Array2i current = scaled_begin;

while (true) {
pixel_mask.push_back(current / subpixel_scale);

if (current == scaled_end) break;

int e2 = 2 * err;
if (e2 > –dy) {
err -= dy;
current.x() += sx;
}
if (e2 < dx) {
err += dx;
current.y() += sy;
}
}

return pixel_mask;
}

5.2 概率更新

贝叶斯更新公式:

$$P(m|z_{1:t})=\frac{P(z_t|m)\cdot P(m|z_{1:t-1})}{P(z_t)}$$

Log-Odds 表示 (数值稳定):

$$\text{logit}(p)=\log \frac{p}{1-p}$$

$$L_t=L_{t-1}+\text{logit}(P(z_t|m))$$

代码实现:

// mapping/2d/probability_grid_range_data_inserter_2d.cc
void ProbabilityGridRangeDataInserter2D::Insert(
const sensor::RangeData& range_data,
GridInterface* grid) {

ProbabilityGrid* probability_grid =
static_cast<ProbabilityGrid*>(grid);

// 1. 更新 Miss 栅格 (自由空间)
for (const Eigen::Vector2f& miss : range_data.misses) {
const Eigen::Array2i cell_index =
probability_grid->limits().GetCellIndex(miss);

UpdateCell(cell_index,
hit_table_[0], // Miss 概率对应的 log-odds
probability_grid);
}

// 2. 更新 Hit 栅格 (障碍物)
for (const Eigen::Vector2f& hit : range_data.returns) {
const Eigen::Array2i cell_index =
probability_grid->limits().GetCellIndex(hit);

UpdateCell(cell_index,
hit_table_[1], // Hit 概率对应的 log-odds
probability_grid);
}
}

void UpdateCell(const Eigen::Array2i& cell_index,
uint16 update_value,
ProbabilityGrid* grid) {

uint16* cell = grid->mutable_value(cell_index);

// Log-Odds 加法 (对应概率乘法)
const int new_value =
ClampToUint16(*cell + update_value – kUpdateMarker);

*cell = new_value;
}

5.3 Submap 生命周期管理

状态转换:

// mapping/internal/2d/local_trajectory_builder_2d.cc
void LocalTrajectoryBuilder2D::AddRangeData(
const sensor::RangeData& range_data) {

// 1. 获取当前活跃 Submap
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
active_submaps_.submaps();

// 2. 插入数据到 Submap
for (const auto& submap : insertion_submaps) {
submap->InsertRangeData(range_data, range_data_inserter_.get());
}

// 3. 检查是否需要完成当前 Submap
if (insertion_submaps.back()->num_range_data() >=
options_.num_range_data()) {

active_submaps_.FinishSubmap(); // kActive → kFinished
}
}

// mapping/internal/2d/active_submaps_2d.cc
void ActiveSubmaps2D::FinishSubmap() {
// 1. 标记当前 Submap 为已完成
submaps_.back()->SetInsertionFinished();

// 2. 创建新的活跃 Submap
AddSubmap(ComputeSubmapPose());
}

6. 运动过滤 (Motion Filter)

目的: 减少冗余 Node，降低后端计算负载。

过滤策略:

// mapping/internal/motion_filter.cc
bool MotionFilter::IsSimilar(const common::Time time,
const transform::Rigid3d& pose) {

if (num_total_ == 0 ||
time – last_time_ > max_time_seconds_ ||
(pose.translation() – last_pose_.translation()).norm() >
max_distance_meters_ ||
transform::GetAngle(pose.rotation() * last_pose_.rotation().inverse()) >
max_angle_radians_) {

// 运动足够大，不过滤
last_time_ = time;
last_pose_ = pose;
++num_total_;
return false;
}

// 运动太小，过滤掉
++num_total_;
++num_different_;
return true;
}

参数配置:

TRAJECTORY_BUILDER_2D.motion_filter = {
max_time_seconds = 5.0, — 5秒后强制插入
max_distance_meters = 0.2, — 移动 20cm 插入
max_angle_radians = 0.004, — 旋转 0.23° 插入
}

过滤率统计:

• 典型过滤率: 70-80% (10Hz LiDAR → 2-3Hz Node 插入)
• 后端计算量降低: ~60%

7. 前端性能分析

7.1 计算时间分布

实测数据 (Intel i7, 2D LiDAR @ 10Hz):

总耗时: ~50ms/frame

├─ 数据预处理: 8ms (16%)
│ ├─ Voxel Filter: 5ms
│ └─ Range Filter: 3ms
│
├─ 位姿预测: 2ms (4%)
│
├─ 扫描匹配: 35ms (70%)
│ ├─ CSM: 25ms ← 瓶颈
│ └─ Ceres: 10ms
│
└─ Submap 插入: 5ms (10%)
├─ Ray Tracing: 3ms
└─ Probability Update: 2ms

7.2 优化建议

1. 减少 CSM 搜索范围

— 从 ±0.3m 减小到 ±0.1m
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher = {
linear_search_window = 0.1, — 默认 0.3
angular_search_window = 0.15, — 默认 0.2
— 加速: 50% → 节省 12ms
}

2. 增大体素尺寸

TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.05 — 默认 0.025
— 点云减少 75% → CSM 加速 40%

3. 禁用 CSM (仅依赖 Ceres)

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
— 风险: 鲁棒性下降 (需要高质量运动先验)

8. 前端失败案例分析

案例 1: 特征贫乏环境

现象: 长走廊、空旷场地匹配失败。

原因:

// 评分函数输出低分
float score = ComputeCandidateScore(...);
// score = 0.45 < min_score (0.6) → 匹配失败

解决方案:

• 降低 min_score 阈值 (风险: 误匹配增加)
• 添加角点检测器 (仅匹配高信息量点)

案例 2: 快速旋转

现象: 原地旋转时位姿跳变。

原因: Motion Filter 过滤了大量旋转数据。

解决方案:

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.001 — 降低阈值

案例 3: 动态物体干扰

现象: 人群中地图出现"幽灵墙"。

解决方案:

// 添加动态物体过滤器 (欧式聚类)
sensor::PointCloud FilterDynamicObjects(const sensor::PointCloud& cloud) {
// 1. 欧式聚类
std::vector<Cluster> clusters = EuclideanClustering(cloud, 0.3);

// 2. 过滤移动簇 (速度 > 0.5m/s)
sensor::PointCloud static_cloud;
for (const auto& cluster : clusters) {
if (EstimateClusterVelocity(cluster) < 0.5) {
static_cloud.insert(static_cloud.end(),
cluster.points.begin(),
cluster.points.end());
}
}

return static_cloud;
}

9. 总结

前端核心算法:

关键参数:

适用场景:

• ✅ 结构化环境 (室内、走廊)
• ✅ 中低速运动 (< 2m/s)
• ⚠️ 高动态场景 (需额外滤波)
• ❌ 完全对称环境 (如空房间)