【Voxel-SLAM】回环检测与位姿图优化（六）

原创 于 2026-04-30 09:56:43 发布 · 413 阅读 · 12 ·
本内容遵循CC 4.0 BY-SA版权协议
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
GEO检测
标签
#算法 #人工智能 #机器学习

04-ALGORITHM_03: 回环检测与位姿图优化 / Loop Closure Detection and Pose Graph Optimization

目录 / Table of Contents

  1. 回环检测概述 / Loop Closure Overview
  2. BTC描述子流水线 / BTC Descriptor Pipeline
  3. 场景检索与验证 / Place Retrieval and Verification
  4. 法向量ICP验证 / Normal ICP Verification
  5. 位姿图优化 / Pose Graph Optimization
  6. 多Session回环 / Multi-session Loop Closure
  7. 回环校正传播 / Loop Correction Propagation
  8. 关键帧管理 / Keyframe Management
  9. HBA全局优化 / HBA Global Bundle Adjustment

1. 回环检测概述 / Loop Closure Overview

1.1 系统架构 / System Architecture

Voxel-SLAM 的回环检测模块运行在独立线程 thd_loop_closure() 中，与里程计线程 thd_odometry_localmapping() 并行执行。其核心流程由三个阶段组成：

  1. BTC 描述子匹配：基于 Binary Triangle Combination (BTC) 描述子进行快速场景检索
  2. 法向量 ICP 验证：对候选回环进行几何一致性精细验证
  3. GTSAM 位姿图优化：使用 ISAM2 增量优化器进行全局位姿校正

The loop closure module in Voxel-SLAM runs in a dedicated thread thd_loop_closure(), parallel to the odometry thread. It consists of three stages:

  1. BTC descriptor matching: Fast place recognition using Binary Triangle Combination descriptors
  2. Normal ICP verification: Geometric consistency check using point-to-plane ICP with normal constraints
  3. GTSAM pose graph optimization: Incremental global pose correction using ISAM2

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2617-2618]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1806-1807]

1.2 整体流程图 / Overall Pipeline

里程计线程 --> buf_lba2loop --> 回环检测线程
                                   |
                         [1] 关键帧累积 (win_size scans)
                                   |
                         [2] BTC描述子生成 (GenerateSTDescs)
                                   |
                         [3] 场景检索 (SearchLoop)
                             |--- candidate_selector()
                             |--- candidate_verify()
                                   |
                         [4] ICP验证 (icp_normal)
                                   |
                         [5] ISAM2优化 (build_graph + isam.update)
                                   |
                         [6] 校正传播 (loop_update)

1.3 线程间通信 / Inter-thread Communication

里程计线程通过 buf_lba2loop 队列将 ScanPose 传递给回环检测线程：

mtx_loop.lock();
buf_lba2loop.push_back(bl);
mtx_loop.unlock();

回环检测完成后，通过设置 loop_detect = 1 通知里程计线程执行 loop_update()：

loop_detect = 1;  // 在thd_loop_closure中设置
// ...
if(loop_detect == 1)
{
    loop_update();  // 在thd_odometry_localmapping中响应
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1659-1661]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2168]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1478-1480]

1.4 关键数据结构 / Key Data Structures

结构文件职责
STDescManagerBTC.h:228BTC描述子管理器
STDBTC.h:73三角形描述子
BinaryDescriptorBTC.h:59二值角点描述子
BTCOctoTreeBTC.h:155BTC体素八叉树
BTCPlaneBTC.h:86BTC平面结构
ScanPoseloop_refine.hpp:17扫描位姿（含点云指针）
PGO_Edgeloop_refine.hpp:163位姿图边
PGO_Edgesloop_refine.hpp:206位姿图边集合
Keyframe(voxel_map.hpp/loop_refine.hpp)关键帧

[VERIFY: VoxelSLAM/src/BTC.h:228]
[VERIFY: VoxelSLAM/src/BTC.h:73-84]
[VERIFY: VoxelSLAM/src/BTC.h:59-63]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:17-45]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:163-204]

2. BTC描述子流水线 / BTC Descriptor Pipeline

2.1 STDescManager 架构 / STDescManager Architecture

STDescManager 是描述子管理的核心类，维护一个哈希表 data_base_ 存储所有历史描述子，以及一个 plane_cloud_vec_ 存储每帧的平面点云：

class STDescManager {
    ConfigSetting config_setting_;
    unsigned int current_frame_id_;
    std::unordered_map<STD_LOC, std::vector<STD>> data_base_;      // 描述子哈希表
    std::vector<pcl::PointCloud<pcl::PointXYZINormal>::Ptr> plane_cloud_vec_;  // 平面点云
};

[VERIFY: VoxelSLAM/src/BTC.h:228-253]

哈希表的键 STD_LOC 基于三角形边长的量化值：

class STD_LOC {
    int64_t x, y, z, a, b, c;
    // hash: ((z * HASH_P) % MAX_N + y) * HASH_P) % MAX_N + x
};

[VERIFY: VoxelSLAM/src/BTC.h:130-153]

2.2 Step 1: init_voxel_map() – 体素化 / Voxelization

将输入点云按 voxel_size_ 划分到体素格中，每个体素对应一个 BTCOctoTree：

void init_voxel_map(const pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud,
                    std::unordered_map<BTCVOXEL_LOC, BTCOctoTree *> &voxel_map)
{
    for (uint i = 0; i < plsize; i++) {
        Eigen::Vector3d p_c(input_cloud->points[i].x, ...);
        double loc_xyz[3];
        for (int j = 0; j < 3; j++) {
            loc_xyz[j] = p_c[j] / config_setting_.voxel_size_;
            if (loc_xyz[j] < 0) loc_xyz[j] -= 1.0;
        }
        BTCVOXEL_LOC position((int64_t)loc_xyz[0], ...);
        // 插入或追加到体素
    }
    // 对每个体素初始化八叉树
    for(const size_t &i: index)
        iter_list[i]->second->init_octo_tree();
}

体素大小由 config_setting_.voxel_size_ 控制（默认 1.0m，高空飞行模式 2.0m）。

[VERIFY: VoxelSLAM/src/BTC.cpp:279-321]
[VERIFY: VoxelSLAM/src/BTC.cpp:11]
[VERIFY: VoxelSLAM/src/BTC.cpp:43]

2.3 init_plane(): 平面拟合 / Plane Fitting

当体素内点数超过 voxel_init_num_（默认 10）时，通过 PCA 拟合平面：

void BTCOctoTree::init_plane() {
    // 计算协方差矩阵
    plane_ptr_->covariance_ = sum(pi * pi^T) / N - center * center^T;
    // 特征值分解
    Eigen::EigenSolver<Eigen::Matrix3d> es(plane_ptr_->covariance_);
    // 最小特征值对应法向量
    if (evalsReal(evalsMin) < config_setting_.plane_detection_thre_) {
        plane_ptr_->normal_ = eigenvectors.col(evalsMin);
        plane_ptr_->is_plane_ = true;
    }
}

平面检测阈值 plane_detection_thre_ 默认为 0.01。当最小特征值小于该阈值时，认为该体素中的点近似共面。

[VERIFY: VoxelSLAM/src/BTC.cpp:90-139]
[VERIFY: VoxelSLAM/src/BTC.h:29]

2.4 Step 2: get_plane() – 提取平面 / Extract Planes

遍历体素地图，收集所有 is_plane_ == true 的平面中心和法向量：

void get_plane(const std::unordered_map<BTCVOXEL_LOC, BTCOctoTree *> &voxel_map,
               pcl::PointCloud<pcl::PointXYZINormal>::Ptr &plane_cloud) {
    for (auto iter = voxel_map.begin(); iter != voxel_map.end(); iter++) {
        if (iter->second->plane_ptr_->is_plane_) {
            pcl::PointXYZINormal pi;
            pi.x = center[0]; pi.y = center[1]; pi.z = center[2];
            pi.normal_x = normal[0]; pi.normal_y = normal[1]; pi.normal_z = normal[2];
            plane_cloud->push_back(pi);
        }
    }
}

平面点云的 header.seq 字段被赋值为帧 ID，用于后续回环检测中定位原始帧。

[VERIFY: VoxelSLAM/src/BTC.cpp:323-338]
[VERIFY: VoxelSLAM/src/BTC.cpp:166]

2.5 get_project_plane(): 投影平面提取 / Projection Plane Extraction

从已检测到的平面中选取主要投影平面。通过判断平面的法向量相似性和距离，将相近的平面合并：

void get_project_plane(std::unordered_map<BTCVOXEL_LOC, BTCOctoTree *> &voxel_map,
                       std::vector<BTCPlane *> &project_plane_list)

合并条件为法向量差的模小于 plane_merge_normal_thre_（默认 0.1）且平面间距离小于 plane_merge_dis_thre_（默认 0.3）：

if (normal_diff.norm() < config_setting_.plane_merge_normal_thre_ ||
    normal_add.norm() < config_setting_.plane_merge_normal_thre_)
    if (dis1 < config_setting_.plane_merge_dis_thre_ &&
        dis2 < config_setting_.plane_merge_dis_thre_)

[VERIFY: VoxelSLAM/src/BTC.cpp:340-458]
[VERIFY: VoxelSLAM/src/BTC.cpp:367-370]

2.6 Step 3: binary_extractor() – 二值描述子提取 / Binary Descriptor Extraction

将点云投影到选定的平面上，构建2D栅格图像，提取具有丰富高度分布的角点：

void binary_extractor(const std::vector<BTCPlane *> proj_plane_list,
                      const pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud,
                      std::vector<BinaryDescriptor> &binary_descriptor_list)

[VERIFY: VoxelSLAM/src/BTC.cpp:571-611]

投影过程 (extract_binary)：

  1. 构建平面的正交坐标系 (x_axis, y_axis, normal)
  2. 将3D点投影到该平面，获取2D坐标和距离
  3. 将2D空间栅格化（分辨率 proj_image_resolution_ = 0.5m）
  4. 对每个栅格，统计不同高度层的占据情况，形成二值占据数组 occupy_array_
  5. summary_ 记录被占据的高度层总数，作为角点显著性度量

[VERIFY: VoxelSLAM/src/BTC.cpp:613-924]

高度层划分：

int cut_num = (dis_threshold_max - dis_threshold_min) / high_inc;

默认：dis_threshold_max = 5m，dis_threshold_min = 0m，high_inc = 0.1m，因此有 50 个高度层。

[VERIFY: VoxelSLAM/src/BTC.h:33-36]

非极大值抑制 (non_maxi_suppression)：

使用 KdTree 在半径 non_max_suppression_radius_（默认 2.0m 或 3.0m）内抑制较弱的角点，只保留 summary_ 最大的：

void non_maxi_suppression(std::vector<BinaryDescriptor> &binary_list) {
    // KdTree radius search
    if (pre_count_list[i] <= pre_count_list[pointIdxRadiusSearch[j]]) {
        is_add_list[i] = false;
    }
}

最终保留 useful_corner_num_ 个角点（默认 100/200）。

[VERIFY: VoxelSLAM/src/BTC.cpp:926-977]
[VERIFY: VoxelSLAM/src/BTC.h:23]

2.7 Step 4: generate_std() – 三角形描述子生成 / Triangle Descriptor Generation

从角点中构建三角形组合，形成 STD (Stable Triangle Descriptor)：

void generate_std(const std::vector<BinaryDescriptor> &binary_list,
                  const int &frame_id, std::vector<STD> &std_list)

对于每个角点，使用 KdTree 找到最近的 K = descriptor_near_num_（默认 15）个邻居，从中选取所有满足边长约束的三角形：

if (a > config_setting_.descriptor_max_len_ || ... ||
    a < config_setting_.descriptor_min_len_ || ...)
    continue;

边长范围：descriptor_min_len_ (2m) 到 descriptor_max_len_ (50m)。

[VERIFY: VoxelSLAM/src/BTC.cpp:979-1126]
[VERIFY: VoxelSLAM/src/BTC.h:43-44]

三角形边长排序：

将三角形三边长排序为 a <= b <= c，并追踪顶点映射关系，确保相同形状的三角形具有一致的描述方式：

if (a > b) { swap; adjust vertex mapping; }
if (b > c) { swap; adjust vertex mapping; }
if (a > b) { swap; adjust vertex mapping; }

退化三角形（近似共线）被过滤：

if (fabs(c - (a + b)) < 0.2) continue;

[VERIFY: VoxelSLAM/src/BTC.cpp:1032-1057]

STD 描述子内容：

typedef struct STD {
    Eigen::Vector3d triangle_;     // 量化后的边长 (scale * a, scale * b, scale * c)
    Eigen::Vector3d angle_;        // 顶点角度信息
    Eigen::Vector3d center_;       // 三角形中心
    int frame_number_;             // 所属帧号
    BinaryDescriptor binary_A_;   // 顶点A的二值描述子
    BinaryDescriptor binary_B_;   // 顶点B的二值描述子
    BinaryDescriptor binary_C_;   // 顶点C的二值描述子
} STD;

其中 triangle_ 的量化尺度 scale = 1.0 / std_side_resolution_（std_side_resolution_ = 0.2m，因此 scale = 5）。

[VERIFY: VoxelSLAM/src/BTC.h:73-84]
[VERIFY: VoxelSLAM/src/BTC.h:46]
[VERIFY: VoxelSLAM/src/BTC.cpp:982]

2.8 GenerateSTDescs(): 完整流水线 / Complete Pipeline

GenerateSTDescs() 按顺序调用上述步骤：

void GenerateSTDescs(pcl::PointCloud<pcl::PointXYZI>::Ptr &input_cloud,
                     std::vector<STD> &stds_vec, int id) {
    // step1: voxelization and plane detection
    init_voxel_map(input_cloud, voxel_map);
    get_plane(voxel_map, plane_cloud);
    plane_cloud->header.seq = id;
    plane_cloud_vec_.push_back(plane_cloud);
    
    // step3: extraction binary descriptors
    get_project_plane(voxel_map, proj_plane_list);
    merge_plane(proj_plane_list, merge_plane_list);
    binary_extractor(merge_plane_list, input_cloud, binary_list);
    
    // step4: generate stable triangle descriptors
    generate_std(binary_list, current_frame_id_, stds_vec);
    
    // step5: clear memory
    for (auto iter = voxel_map.begin(); ...) delete(iter->second);
}

[VERIFY: VoxelSLAM/src/BTC.cpp:156-203]

2.9 AddSTDescs(): 添加到数据库 / Add to Database

void AddSTDescs(const std::vector<STD> &stds_vec) {
    current_frame_id_++;
    for (auto &single_std : stds_vec) {
        STD_LOC position;
        position.x = (int)(single_std.triangle_[0] + 0.5);
        position.y = (int)(single_std.triangle_[1] + 0.5);
        position.z = (int)(single_std.triangle_[2] + 0.5);
        auto iter = data_base_.find(position);
        if (iter != data_base_.end())
            iter->second.push_back(single_std);
        else
            data_base_[position] = {single_std};
    }
}

每个 STD 按照其边长的四舍五入整数值作为哈希键，存入数据库。这样形状相似的三角形会落入相同或相邻的哈希桶中。

[VERIFY: VoxelSLAM/src/BTC.cpp:258-277]

3. 场景检索与验证 / Place Retrieval and Verification

3.1 SearchLoop() 概述 / SearchLoop Overview

void SearchLoop(std::vector<STD> &stds_vec, std::pair<int, double> &loop_result,
                std::pair<Eigen::Vector3d, Eigen::Matrix3d> &loop_transform,
                std::vector<std::pair<STD, STD>> &loop_std_pair,
                pcl::PointCloud<pcl::PointXYZINormal>::Ptr pl_cur)

[VERIFY: VoxelSLAM/src/BTC.cpp:205-256]

流程：

  1. 调用 candidate_selector() 进行粗匹配，选出候选帧
  2. 对每个候选帧调用 candidate_verify() 进行几何验证
  3. 选取得分最高且超过 icp_threshold_（默认 0.15）的候选帧作为回环

3.2 candidate_selector(): 候选帧选择 / Candidate Frame Selection

void candidate_selector(std::vector<STD> &current_STD_list,
                        std::vector<STDMatchList> &candidate_matcher_vec)

[VERIFY: VoxelSLAM/src/BTC.cpp:1128-1279]

粗匹配过程：

  1. 对当前帧的每个 STD 描述子，在哈希表的 3x3x3 邻域中搜索形状相似的历史描述子：
for (auto &voxel_inc : voxel_round)  // 27个邻域
{
    position.x = (int)(descriptor.triangle_[0] + voxel_inc[0]);
    // ...
    auto iter = data_base_.find(position);
    if (iter != data_base_.end()) {
        for (size_t j = 0; j < iter->second.size(); j++) {
            // 跳过时间相近的帧
            if ((descriptor.frame_number_ - iter->second[j].frame_number_) 
                > config_setting_.skip_near_num_) {
                double dis = (descriptor.triangle_ - iter->second[j].triangle_).norm();
                if (dis < dis_threshold) {
                    // 计算二值描述子相似度
                    double similarity = (binary_similarity(A, A') + 
                                        binary_similarity(B, B') + 
                                        binary_similarity(C, C')) / 3;
                    if (similarity > config_setting_.similarity_threshold_)
                        // 记录匹配
                }
            }
        }
    }
}

[VERIFY: VoxelSLAM/src/BTC.cpp:1159-1211]

  1. skip_near_num_ 控制跳过时间上相近的帧（默认 30），防止与相邻帧匹配。对于历史 session，该值被设为负数以允许所有匹配：
std_manager->config_setting_.skip_near_num_ = -(std_manager->plane_cloud_vec_.size()+10);

[VERIFY: VoxelSLAM/src/BTC.h:49]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:401]

  1. 对匹配计数进行投票，选出最多 candidate_num_（默认 20/100）个候选帧（需要至少 5 票）：
for (int cnt = 0; cnt < config_setting_.candidate_num_; cnt++) {
    double max_vote_index = std::max_element(match_array.begin(), match_array.end()) - match_array.begin();
    if (max_vote >= 5) {
        // 添加候选帧
        match_array[max_vote_index] = 0;
    }
}

[VERIFY: VoxelSLAM/src/BTC.cpp:1235-1278]

3.3 binary_similarity(): 二值描述子相似度 / Binary Descriptor Similarity

double binary_similarity(const BinaryDescriptor &b1, const BinaryDescriptor &b2) {
    double dis = 0;
    for (size_t i = 0; i < b1.occupy_array_.size(); i++) {
        if (b1.occupy_array_[i] == true && b2.occupy_array_[i] == true)
            dis += 1;
    }
    return 2 * dis / (b1.summary_ + b2.summary_);
}

使用修改版 Dice 系数：2 * |A ∩ B| / (|A| + |B|)，值域为 [0, 1]。相似度阈值 similarity_threshold_ 默认为 0.7。

[VERIFY: VoxelSLAM/src/BTC.cpp:70-80]
[VERIFY: VoxelSLAM/src/BTC.h:52]

3.4 candidate_verify(): 候选帧几何验证 / Candidate Frame Geometric Verification

void candidate_verify(STDMatchList &candidate_matcher, double &verify_score,
                      std::pair<Eigen::Vector3d, Eigen::Matrix3d> &relative_pose,
                      std::vector<std::pair<STD, STD>> &sucess_match_list,
                      pcl::PointCloud<pcl::PointXYZINormal>::Ptr pl_cur)

[VERIFY: VoxelSLAM/src/BTC.cpp:1281-1396]

RANSAC 风格的验证过程：

  1. 对候选匹配列表中的每对匹配三角形，调用 triangle_solver() 计算变换
  2. 用该变换验证其他匹配对（投票机制），距离阈值 3m：
Eigen::Vector3d A_transform = test_rot * verify_pair.first.binary_A_.location_ + test_t;
if((A_transform - verify_pair.second.binary_A_.location_).norm() >= dis_threshold)
    continue;
// ... 类似检查 B 和 C
vote++;
  1. 选择投票数最多的变换（需至少 4 票）
  2. 最终通过 plane_geometric_verify() 计算平面对齐得分

[VERIFY: VoxelSLAM/src/BTC.cpp:1307-1349]
[VERIFY: VoxelSLAM/src/BTC.cpp:1361]

3.5 triangle_solver(): 三角形求解 / Triangle Transform Solver

使用 SVD 分解计算两个三角形之间的刚体变换：

void triangle_solver(std::pair<STD, STD> &std_pair, Eigen::Vector3d &t, Eigen::Matrix3d &rot) {
    Eigen::Matrix3d src, ref;
    src.col(0) = std_pair.first.binary_A_.location_ - std_pair.first.center_;
    // ... B, C类似
    ref.col(0) = std_pair.second.binary_A_.location_ - std_pair.second.center_;
    // ... B, C类似
    
    Eigen::Matrix3d covariance = src * ref.transpose();
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(covariance, ComputeThinU | ComputeThinV);
    rot = V * U.transpose();
    if (rot.determinant() < 0) {
        rot = V * diag(1,1,-1) * U.transpose();
    }
    t = -rot * std_pair.first.center_ + std_pair.second.center_;
}

这是经典的 Procrustes 分析/点对配准方法。当 det(R) < 0 时（反射情况），通过翻转最后一个奇异值来修正。

[VERIFY: VoxelSLAM/src/BTC.cpp:1398-1420]

3.6 plane_geometric_verify(): 平面几何验证 / Plane Geometric Verification

计算源平面云经过变换后与目标平面云的匹配比例：

double plane_geometric_verify(
    const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &source_cloud,
    const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &target_cloud,
    const std::pair<Eigen::Vector3d, Eigen::Matrix3d> &transform)

对每个源平面点：

  1. 变换位置和法向量
  2. KdTree 最近邻搜索
  3. 检查法向量一致性和点到面距离

匹配条件：

if ((normal_inc.norm() < normal_threshold ||
     normal_add.norm() < normal_threshold) &&
    point_to_plane < dis_threshold)
    useful_match++;

返回值为匹配比例 useful_match / source_cloud->size()。

[VERIFY: VoxelSLAM/src/BTC.cpp:1422-1479]
[VERIFY: VoxelSLAM/src/BTC.h:55-56]

4. 法向量ICP验证 / Normal ICP Verification

4.1 icp_normal() 概述 / Overview

icp_normal() 是回环检测中最关键的几何验证步骤。在 BTC 描述子提供粗略变换后，ICP 进一步精化对齐并验证匹配质量：

bool icp_normal(pcl::PointCloud<PointType> &pl_src, 
                pcl::PointCloud<PointType> &pl_tar,
                pair<Eigen::Vector3d, Eigen::Matrix3d> &pose, 
                double icp_eigval)

返回 true 表示回环有效，false 表示回环被拒绝。

[VERIFY: VoxelSLAM/src/loop_refine.hpp:47]

4.2 KdTree 构建 / KdTree Construction

以目标点云构建 KdTree：

pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;
pcl::PointCloud<pcl::PointXYZ> input_cloud;
for(PointType &ap: pl_tar.points) {
    pcl::PointXYZ pi;
    pi.x = ap.x; pi.y = ap.y; pi.z = ap.z;
    input_cloud.push_back(pi);
}
kd_tree.setInputCloud(input_cloud.makeShared());

[VERIFY: VoxelSLAM/src/loop_refine.hpp:49-57]

4.3 两阶段收敛策略 / Two-Phase Convergence Strategy

ICP 采用从粗到精的两阶段策略：

Phase 1 (Coarse)：

Eigen::Vector4d paras(0.2, 0.2, 0.5, 3);
  • 法向量差异阈值: 0.2
  • 法向量和阈值: 0.2
  • 点到面距离阈值: 0.5m
  • 点到点距离阈值: 3m

Phase 2 (Fine) – 首次收敛后切换：

if(dxi.head(3).norm() < 1e-3 && dxi.tail(3).norm() < 1e-3) {
    if(is_converge)
        break;
    else {
        paras << 0.1, 0.1, 0.1, 1;
        is_converge = 1;
    }
}
  • 法向量差异阈值: 0.1
  • 法向量和阈值: 0.1
  • 点到面距离阈值: 0.1m
  • 点到点距离阈值: 1m

[VERIFY: VoxelSLAM/src/loop_refine.hpp:62]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:120-130]

4.4 匹配条件 / Matching Conditions

Eigen::Vector3d normal_inc = ni - tni;
Eigen::Vector3d normal_add = ni + tni;
double point_to_point_dis = (pi - tpi).norm();
double point_to_plane = fabs(tni.transpose() * (pi - tpi));
if ((normal_inc.norm() < paras[0] || normal_add.norm() < paras[1]) 
    && point_to_plane < paras[2] && point_to_point_dis < paras[3])

法向量条件使用"或"逻辑：normal_inc 和 normal_add 分别对应法向量同向和反向的情况（因为法向量方向可能翻转）。

[VERIFY: VoxelSLAM/src/loop_refine.hpp:93-97]

4.5 最小二乘求解 / Least Squares Solution

对匹配的点对构建点到面的最小二乘问题：

double rr = tni.dot(pi - tpi);
Eigen::Matrix<double, 6, 1> jac;
jac.head(3) = hat(plocal) * pose.second.transpose() * tni;  // 对旋转
jac.tail(3) = tni;                                            // 对平移

Hess += jac * jac.transpose();
JacT += jac * rr;
resi += 0.5 * rr * rr;

Eigen::Matrix<double, 6, 1> dxi = Hess.ldlt().solve(-JacT);
pose.second = pose.second * Exp(dxi.head(3));
pose.first = pose.first + dxi.tail(3);

最多迭代 20 次。

[VERIFY: VoxelSLAM/src/loop_refine.hpp:100-117]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:69]

4.6 特征值验证 / Eigenvalue Validation

法向量矩阵的特征值用于判断约束是否充分：

Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(mat_norm);
Eigen::Vector3d eig_vec = saes.eigenvalues();
return eig_vec[0] > icp_eigval && is_converge == 1;

mat_norm 累积了所有匹配法向量的外积 sum(n * n^T)。最小特征值 eig_vec[0] 必须大于 icp_eigval（默认 14），确保三个方向上都有足够的约束。

[VERIFY: VoxelSLAM/src/loop_refine.hpp:139-142]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1813]

4.7 在回环检测中的调用 / Invocation in Loop Detection

if(search_result.first >= 0 && search_result.second > juds[id])
{
    if(icp_normal(*(std_manager->plane_cloud_vec_.back()), 
                  *(std_managers[id]->plane_cloud_vec_[search_result.first]), 
                  loop_transform, icp_eigval))
    {
        // 回环确认，计算漂移并添加边
    }
}

只有同时满足 BTC 得分超过阈值（juds[id]，默认 0.45）和 ICP 验证通过，才确认回环。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1997-1999]

5. 位姿图优化 / Pose Graph Optimization

5.1 GTSAM ISAM2 架构 / GTSAM ISAM2 Architecture

系统使用 GTSAM 库的 ISAM2 增量优化器进行位姿图优化：

gtsam::ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
gtsam::ISAM2 isam(parameters);
isam.update(graph, initial);
for(int i = 0; i < 5; i++) isam.update();
gtsam::Values results = isam.calculateEstimate();

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2090-2097]

5.2 add_edge(): 创建BetweenFactor / Creating BetweenFactor

add_edge() 有两个重载版本，分别接受 IMUST 状态或直接的旋转/平移：

版本 1：从两个IMUST状态创建相对变换

void add_edge(int pos1, int pos2, IMUST &x1, IMUST &x2, 
              gtsam::NonlinearFactorGraph &graph, 
              gtsam::noiseModel::Diagonal::shared_ptr odometryNoise)
{
    gtsam::Point3 tt(x1.R.transpose() * (x2.p - x1.p));  // 相对平移（x1坐标系下）
    gtsam::Rot3 RR(x1.R.transpose() * x2.R);               // 相对旋转
    gtsam::NonlinearFactor::shared_ptr factor(
        new gtsam::BetweenFactor<gtsam::Pose3>(pos1, pos2, gtsam::Pose3(RR, tt), odometryNoise));
    graph.push_back(factor);
}

[VERIFY: VoxelSLAM/src/loop_refine.hpp:147-153]

版本 2：直接指定旋转和平移

void add_edge(int pos1, int pos2, Eigen::Matrix3d &rot, Eigen::Vector3d &tra,
              gtsam::NonlinearFactorGraph &graph, 
              gtsam::noiseModel::Diagonal::shared_ptr odometryNoise)
{
    gtsam::Point3 tt(tra);
    gtsam::Rot3 RR(rot);
    gtsam::NonlinearFactor::shared_ptr factor(
        new gtsam::BetweenFactor<gtsam::Pose3>(pos1, pos2, gtsam::Pose3(RR, tt), odometryNoise));
    graph.push_back(factor);
}

[VERIFY: VoxelSLAM/src/loop_refine.hpp:155-161]

5.3 build_graph(): 构建多Session位姿图 / Building Multi-session Pose Graph

void build_graph(gtsam::Values &initial, gtsam::NonlinearFactorGraph &graph,
                 int cur_id, PGO_Edges &lp_edges, 
                 gtsam::noiseModel::Diagonal::shared_ptr default_noise,
                 vector<int> &ids, vector<int> &stepsizes, int lpedge_enable)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1741]

构建步骤：

  1. 连通图发现：通过 lp_edges.connect(cur_id, ids) 找到与当前 session 连通的所有 session：
ids.clear();
lp_edges.connect(cur_id, ids);

PGO_Edges::connect() 使用 BFS/DFS 遍历图的连通分量：

void connect(int root, vector<int> &ids) {
    ids.clear();
    ids.push_back(root);
    tras(root, ids);     // 递归遍历
    sort(ids.begin(), ids.end());
}

[VERIFY: VoxelSLAM/src/loop_refine.hpp:237-243]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1744-1745]

  1. 计算全局索引偏移：
stepsizes.clear(); stepsizes.push_back(0);
for(int i = 0; i < ids.size(); i++)
    stepsizes.push_back(stepsizes.back() + multimap_scanPoses[ids[i]]->size());

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1747-1749]

  1. 添加里程计边（相邻帧之间的 BetweenFactor）：
for(int j = bsize; j < stepsizes[ii+1]; j++) {
    initial.insert(j, pose3);
    if(j > bsize) {
        gtsam::Vector samv6(6);
        samv6 = multimap_scanPoses[ids[ii]]->at(j-1-bsize)->v6;
        gtsam::noiseModel::Diagonal::shared_ptr v6_noise = 
            gtsam::noiseModel::Diagonal::Variances(samv6);
        add_edge(j-1, j, ..., graph, v6_noise);
    }
}

噪声模型使用每个 ScanPose 的 v6 字段，该字段源自 Local BA 的 Hessian 矩阵对角线：

bl->v6 = hess.block<6, 6>(0, DIM).diagonal();
for(int i = 0; i < 6; i++) bl->v6[i] = 1.0 / fabs(bl->v6[i]);

这意味着噪声与 Hessian 对角元素成反比 – Hessian 越大（优化约束越强），噪声越小。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1751-1767]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1657-1658]

  1. 添加先验因子：对第一个 session 的首个位姿添加强先验：
Eigen::Matrix<double, 6, 1> v6_fixd;
v6_fixd << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9;
gtsam::noiseModel::Diagonal::shared_ptr fixd_noise = 
    gtsam::noiseModel::Diagonal::Variances(gtsam::Vector(v6_fixd));
graph.addPrior(0, pose3, fixd_noise);

极小的方差（1e-9）使得第一个位姿几乎被固定。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1770-1783]

  1. 添加回环边（如果 lpedge_enable == 1）：
if(lpedge_enable == 1)
for(PGO_Edge &edge: lp_edges.edges) {
    vector<int> step(2);
    if(edge.is_adapt(ids, step)) {
        for(int i = 0; i < edge.rots.size(); i++) {
            int id1 = mp[0] + edge.ids1[i];
            int id2 = mp[1] + edge.ids2[i];
            add_edge(id1, id2, edge.rots[i], edge.tras[i], graph, default_noise);
        }
    }
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1786-1801]

5.4 PGO_Edge / PGO_Edges: 位姿图边管理 / Pose Graph Edge Management

struct PGO_Edge {
    int m1, m2;                 // 两个session的ID
    vector<int> ids1, ids2;     // 帧ID列表
    PLM(3) rots;                // 相对旋转列表
    PLV(3) tras;                // 相对平移列表
    PLV(6) covs;                // 协方差列表
};

PGO_Edges 维护所有边并提供连通性查询：

struct PGO_Edges {
    vector<PGO_Edge> edges;
    vector<vector<int>> mates;   // 邻接表
    void push(m1, m2, id1, id2, rot, tra, v6);  // 添加边
    void connect(root, ids);      // 查找连通分量
};

[VERIFY: VoxelSLAM/src/loop_refine.hpp:163-204]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:206-267]

5.5 优化结果应用 / Applying Optimization Results

优化完成后，将 ISAM2 的结果写回到各个 session 的 ScanPose：

gtsam::Values results = isam.calculateEstimate();
for(int ii = 0; ii < idsize; ii++) {
    int tip = ids[ii];
    for(int j = stepsizes[ii]; j < stepsizes[ii+1]; j++) {
        int ord = j - stepsizes[ii];
        multimap_scanPoses[tip]->at(ord)->set_state(results.at(j).cast<gtsam::Pose3>());
    }
}

ScanPose::set_state() 不仅更新位姿，还旋转速度向量：

void set_state(const gtsam::Pose3 &pose) {
    Eigen::Matrix3d rot = pose.rotation().matrix();
    rot = rot * x.R.transpose();   // 计算修正旋转
    x.R = pose.rotation().matrix();
    x.p = pose.translation();
    x.v = rot * x.v;               // 相应旋转速度
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2100-2112]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:36-43]

6. 多Session回环 / Multi-session Loop Closure

6.1 previous_map_read(): 加载历史Session / Loading Historical Sessions

void previous_map_read(vector<STDescManager*> &std_managers, 
                       vector<vector<ScanPose*>*> &multimap_scanPoses,
                       vector<vector<Keyframe*>*> &multimap_keyframes,
                       ConfigSetting &config_setting, PGO_Edges &edges,
                       ros::NodeHandle &n, vector<string> &fnames,
                       vector<double> &juds, string &savepath, int win_size)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:307]

加载流程：

  1. 读取位姿文件：read_lidarstate(fname+"/alidarState.txt", *bl_tem)
  2. 加载 PCD 点云并组装关键帧（每 win_size 个扫描合并一个关键帧）
  3. 为每个关键帧生成 BTC 描述子
  4. 设置 skip_near_num_ 为负数以允许跨 session 完全匹配

[VERIFY: VoxelSLAM/src/voxelslam.cpp:313-404]

关键帧组装：

for(int j = 0; j < win_size; j++) {
    Eigen::Vector3d delta_p = xc.R.transpose() * (xxbuf[j].p - xc.p);
    Eigen::Matrix3d delta_R = xc.R.transpose() * xxbuf[j].R;
    for(pcl::PointXYZI pp: plbuf[j]->points) {
        Eigen::Vector3d v3(pp.x, pp.y, pp.z);
        v3 = delta_R * v3 + delta_p;
        // ...
    }
}

将 win_size 个扫描的点变换到最后一个扫描的坐标系下合并。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:348-360]

BTC 描述子生成（每 mgsize 个关键帧一组）：

for(int i = 0; i+acsize < subsize; i += mgsize) {
    int up = i + acsize;
    // 合并acsize个关键帧的点云
    std_manager->GenerateSTDescs(pl_btc, stds_vec, keyframes_tem->at(up-1)->id);
    std_manager->AddSTDescs(stds_vec);
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:377-400]

6.2 跨Session回环检测 / Cross-session Loop Detection

在 thd_loop_closure() 的主循环中，对所有 session（包括当前和历史 session）执行检索：

for(int id = 0; id <= cur_id; id++) {
    std_managers[id]->SearchLoop(stds_vec, search_result, loop_transform, loop_std_pair, 
                                 std_manager->plane_cloud_vec_.back());
    if(search_result.first >= 0 && search_result.second > juds[id]) {
        if(icp_normal(...)) {
            // 根据 id == cur_id 判断是自身回环还是跨session回环
            if(id == cur_id) {
                // 自身session回环，检查漂移比
                double span = smp->jour - keyframes->at(search_result.first)->jour;
                if(drift_p / span < ratio_drift)
                    isPush = true;
            } else {
                // 跨session回环，需要重建图
                isGraph = true;
                isOpt = true;
                g_update = 1;
            }
        }
    }
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1987-2079]

6.3 漂移检测 / Drift Detection

对自身 session 的回环，使用漂移比阈值来过滤误匹配：

double drift_p = (xx.R * loop_transform.first + xx.p - xc.p).norm();
// ...
if(drift_p / span < ratio_drift)  // ratio_drift 默认 0.05
    isPush = true;

漂移距离与行驶距离之比小于 5% 才接受。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2004]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2012-2014]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1813]

对历史 session 的回环，使用累积距离 jours[id] 作为判断依据：

if(drift_p / jours[id] < 0.05)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2044]

6.4 频率控制 / Frequency Control

通过 relc_counts 计数器控制回环优化的频率：

vector<int> relc_counts(std_managers.size(), prev_halt);
// ...
if(relc_counts[id] > curr_halt && drift_p > 0.10) {
    isOpt = true;
    for(int &cnt: relc_counts) cnt = 0;
}
// ...
for(int &it: relc_counts) it++;

curr_halt = 10（自身session）和 prev_halt = 30（历史session）控制两次优化之间的最小关键帧间隔。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1814-1815]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2018-2021]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2080]

7. 回环校正传播 / Loop Correction Propagation

7.1 loop_update() 概述 / Overview

当回环检测线程设置 loop_detect = 1 后，里程计线程在下一次循环中调用 loop_update() 来应用校正：

void loop_update()

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1101]

7.2 dx 变换计算 / Computing dx Transform

在回环检测线程中，优化完成后计算当前帧前后的变换差：

IMUST x1 = scanPoses->at(buf_base-1)->x;  // 优化前的位姿
// ... ISAM2优化 ...
IMUST x3 = scanPoses->at(buf_base-1)->x;  // 优化后的位姿
dx.p = x3.p - x3.R * x1.R.transpose() * x1.p;
dx.R = x3.R * x1.R.transpose();

dx 表示从优化前坐标系到优化后坐标系的刚体变换。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2100]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2126-2128]

7.3 体素地图重建 / Voxel Map Reconstruction

// 清除当前体素地图
for(auto iter = surf_map.begin(); iter != surf_map.end(); iter++) {
    iter->second->tras_ptr(octos_release);
    iter->second->clear_slwd(sws[0]);
    delete iter->second;
}
surf_map.clear(); surf_map_slide.clear();
// 使用回环检测线程预构建的新地图
surf_map = map_loop;
map_loop.clear();

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1105-1114]

7.4 滑动窗口状态校正 / Sliding Window State Correction

对已发送到回环检测但尚未优化的帧（buf_lba2loop）应用 ScanPose::update(dx)：

for(ScanPose *bl: buf_lba2loop) {
    bl->update(dx);
}

ScanPose::update() 实现：

void update(IMUST dx) {
    x.v = dx.R * x.v;
    x.p = dx.R * x.p + dx.p;
    x.R = dx.R * x.R;
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1129-1131]
[VERIFY: VoxelSLAM/src/loop_refine.hpp:29-34]

对当前滑动窗口中的帧直接变换：

for(int i = 0; i < win_count; i++) {
    IMUST &x = x_buf[i];
    x.v = dx.R * x.v;
    x.p = dx.R * x.p + dx.p;
    x.R = dx.R * x.R;
    if(g_update == 1)
        x.g = dx.R * x.g;
}

如果是跨 session 回环（g_update == 1），还需要旋转重力向量。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1138-1149]

7.5 地图重建与填充 / Map Rebuild and Population

校正后将所有帧的点重新插入体素地图：

// 插入 buf_lba2loop 中的点
for(ScanPose *bl: buf_lba2loop) {
    IMUST xx = bl->x;
    PVec pvec_tem = *(bl->pvec);
    for(pointVar &pv: pvec_tem)
        pv.pnt = xx.R * pv.pnt + xx.p;
    cut_voxel(surf_map, pvec_tem, win_size, 0);
}

// 插入滑动窗口中的点
for(int i = 0; i < win_count; i++) {
    pwld.clear();
    for(pointVar &pv: *pvec_buf[i])
        pwld.push_back(x_buf[i].R * pv.pnt + x_buf[i].p);
    cut_voxel(surf_map, pvec_buf[i], i, surf_map_slide, win_size, pwld, sws[0]);
}

// 重新切割体素
for(auto iter = surf_map.begin(); iter != surf_map.end(); ++iter)
    iter->second->recut(win_count, x_buf, sws[0]);

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1161-1180]

7.6 重力更新状态机 / Gravity Update State Machine

g_update 使用三态机制：

  • g_update = 0: 正常模式
  • g_update = 1: 跨session回环触发，需要旋转重力
  • g_update = 2: loop_update() 完成后设置，触发 Local BA 中的重力优化
if(g_update == 1) g_update = 2;  // loop_update末尾
// ...
if(g_update == 2) {
    LI_BA_OptimizerGravity opt_lsv;
    opt_lsv.damping_iter(x_buf, voxhess, imu_pre_buf, resis, &hess, 5);
    g_update = 0;
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1182]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1641-1649]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:2038]

8. 关键帧管理 / Keyframe Management

8.1 关键帧累积策略 / Keyframe Accumulation Strategy

在回环检测线程中，每 win_size 个扫描帧累积为一个关键帧：

if(bl_local.size() < win_size) continue;

同时还需要满足角度/距离阈值（防止低运动时过于频繁地创建关键帧）：

double ang = Log(x_key.R.transpose() * xc.R).norm() * 57.3;
double len = (xc.p - x_key.p).norm();
if(ang < 5 && len < 0.1 && buf_base > win_size)
{
    bl_local.front()->pvec = nullptr;
    bl_local.pop_front();
    continue;
}

旋转超过 5 度或平移超过 0.1m 时创建新关键帧。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1931-1939]

8.2 关键帧结构 / Keyframe Structure

Keyframe {
    IMUST x0;              // 原始位姿
    int id;                // 在ScanPose序列中的索引
    double jour;           // 累积行驶距离
    int mp;                // 所属session ID (用于HBA)
    pcl::PointCloud<PointType>::Ptr plptr;  // 降采样点云
    bool exist;            // 是否已加载到地图
}

关键帧的点云通过 down_sampling_pvec() 降采样后存储：

down_sampling_pvec(*pptr, voxel_size/10, *(smp->plptr));

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1962-1963]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1965]

8.3 关键帧合并点云 / Keyframe Point Cloud Merging

对于 win_size 个扫描帧，将它们变换到最后一帧的坐标系下合并：

for(int i = 0; i < win_size; i++) {
    ScanPose &bl = *bl_local[i];
    Eigen::Vector3d delta_p = xc.R.transpose() * (bl.x.p - xc.p);
    Eigen::Matrix3d delta_R = xc.R.transpose() * bl.x.R;
    for(pointVar pv: *(bl.pvec)) {
        pv.pnt = delta_R * pv.pnt + delta_p;
        pptr->push_back(pv);
    }
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1945-1955]

8.4 keyframe_loading(): KdTree历史关键帧加载 / Historical Keyframe Loading

keyframe_loading() 使用 KdTree 在关键帧位置上进行半径搜索，将附近的历史关键帧加载到当前体素地图中：

void keyframe_loading(double jour)
{
    if(history_kfsize <= 0) return;
    PointType ap_curr;
    ap_curr.x = x_curr.p[0]; ap_curr.y = x_curr.p[1]; ap_curr.z = x_curr.p[2];
    vector<int> vec_idx;
    vector<float> vec_dis;
    kd_keyframes.radiusSearch(ap_curr, 10, vec_idx, vec_dis);  // 半径10m
    
    for(int id: vec_idx) {
        if(keyframes->at(id)->exist) {
            // 加载点云到体素地图
            Keyframe &kf = *(keyframes->at(id));
            // ... 变换并插入 cut_voxel
            kf.exist = 0;
            history_kfsize--;
            break;  // 每次只加载一个
        }
    }
}

每次只加载一个最近的关键帧，避免一次性加载过多导致延迟。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:1189-1228]

8.5 KdTree 更新 / KdTree Update

回环优化后更新 KdTree：

pl_kdmap->clear();
for(int i = 0; i < subsize-init_num; i++) {
    Keyframe &kf = *(keyframes->at(i));
    kf.exist = 1;
    PointType pp;
    pp.x = kf.x0.p[0]; pp.y = kf.x0.p[1]; pp.z = kf.x0.p[2];
    pp.intensity = cur_id; pp.curvature = i;
    pl_kdmap->push_back(pp);
}
kd_keyframes.setInputCloud(pl_kdmap);
history_kfsize = pl_kdmap->size();

最近的 init_num（= 5）个关键帧不进入 KdTree（它们的点已在 map_loop 中预构建）。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2152-2167]

9. HBA全局优化 / HBA Global Bundle Adjustment

9.1 概述 / Overview

Voxel-SLAM 采用 Hierarchical Bundle Adjustment (HBA) 方法进行全局优化。HBA 运行在独立线程 thd_globalmapping() 中，分为两层：

  1. Bottom-up (自底向上)：在局部关键帧窗口中进行 point-to-plane BA，提取帧间约束
  2. Top-down (自顶向下)：利用提取的约束进行位姿图优化
main()
  |-> thread thd_loop_closure    // 回环检测
  |-> thread thd_globalmapping   // HBA全局优化
  |-> thd_odometry_localmapping  // 里程计 (主线程)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2617-2619]

9.2 Bottom-up: HBA_add_edge() / Bottom-up Edge Extraction

void HBA_add_edge(vector<IMUST> &p_xs, vector<Keyframe*> &p_smps, 
                  PGO_Edges &gba_edges, vector<int> &maps, 
                  int max_iter, int thread_num, 
                  pcl::PointCloud<PointType>::Ptr plptr = nullptr)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2320]

流程：

  1. 将关键帧的点云放入 OctreeGBA 体素地图
  2. 对体素进行 recut 提取平面约束
  3. 运行 Lidar_BA_Optimizer::damping_iter() 优化位姿
  4. 从优化后的 Hessian 矩阵提取帧间约束

约束提取：

for(int i = 0; i < wdsize - 1; i++)
for(int j = i+1; j < wdsize; j++) {
    bool isAdd = true;
    Eigen::Matrix<double, 6, 1> v6;
    for(int k = 0; k < 6; k++) {
        double hc = fabs(hess(6*i+k, 6*j+k));
        if(hc < 1e-6) { isAdd = false; break; }
        v6[k] = 1.0 / hc;
    }
    if(isAdd) {
        Eigen::Vector3d tra = xs[i].R.transpose() * (xs[j].p - xs[i].p);
        Eigen::Matrix3d rot = xs[i].R.transpose() * xs[j].R;
        gba_edges.push(s1.mp, s2.mp, s1.id, s2.id, rot, tra, v6);
    }
}

Hessian 的 off-diagonal 块 hess(6*i+k, 6*j+k) 的绝对值必须大于 1e-6，否则说明两帧之间缺乏足够的共视约束。噪声为 Hessian 对角的倒数。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2405-2427]

9.3 OctreeGBA: HBA体素地图 / HBA Voxel Map

OctreeGBA 是专为 HBA 设计的八叉树结构，支持多帧点云的联合管理：

class OctreeGBA {
    vector<PLV(3)> locals, worlds;  // 每帧的局部和世界坐标点
    PointCluster pcr_add;            // 聚合统计量
    int layer, octo_state, wdsize;
    OctreeGBA* leaves[8];
};

[VERIFY: VoxelSLAM/src/loop_refine.hpp:273-296]

recut() 方法判断平面并提取优化约束：

void recut(LidarFactor &vox_opt, vector<OctreeGBA*> &oct_buf) {
    if(pcr_add.N <= 10) return;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(pcr_add.cov());
    is_plane = plane_judge(eig_value);
    if(is_plane) {
        // 提取约束
        vox_opt.push_voxel(pcrs, pcr_fix, coe, eig_value, eig_vector, pcr_add);
    } else if(layer < max_layer) {
        subdivide(oct_buf);  // 继续细分
    }
}

[VERIFY: VoxelSLAM/src/loop_refine.hpp:358-404]

多线程 recut：

void OctreeGBA_multi_recut(unordered_map<VOXEL_LOC, OctreeGBA*> &feat_map, 
                           LidarFactor &voxhess, int thd_num)

[VERIFY: VoxelSLAM/src/loop_refine.hpp:483-537]

9.4 Top-down: topDownProcess() / Top-down Optimization

void topDownProcess(gtsam::Values &initial, gtsam::NonlinearFactorGraph &graph, 
                    vector<int> &ids, vector<int> &stepsizes)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2231]

流程：

  1. 等待 bottom-up 完成（while(gba_flag)）
  2. 将 gba_edges1（单session内）和 gba_edges2（跨session）的边添加到图中
  3. 运行 ISAM2 优化
  4. 将结果应用到所有 ScanPose 和 Keyframe
for(PGO_Edge &edge: gba_edges1.edges) {
    // 添加单session内的HBA约束
}
for(PGO_Edge &edge: gba_edges2.edges) {
    // 添加跨session的HBA约束
}
gtsam::ISAM2 isam(parameters);
isam.update(graph, initial);
for(int i = 0; i < 5; i++) isam.update();
gtsam::Values results = isam.calculateEstimate();

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2247-2287]

9.5 thd_globalmapping(): 全局建图线程 / Global Mapping Thread

void thd_globalmapping(ros::NodeHandle &n)

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2485]

该线程持续监听关键帧的生成，每 wdsize（= 10）个关键帧执行一次 bottom-up 优化：

localID.push_back(buf_base);
if(localID.size() < wdsize) continue;

// 提取局部帧
vector<IMUST> xs;
vector<Keyframe*> smp_local;
for(int i: localID) {
    xs.push_back(multimap_keyframes[smp_mp]->at(i)->x0);
    smp_local.push_back(multimap_keyframes[smp_mp]->at(i));
}

// 执行HBA_add_edge
HBA_add_edge(xs, smp_local, gba_edges1, mps, 1, 2, gba_smp->plptr);

滑动窗口步长为 mgsize（= 5），因此有 50% 的重叠。

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2530-2557]

当系统结束（is_finish && gba_size <= buf_base）时，执行最终的跨session全局优化：

if(total_ba == 1) {
    HBA_add_edge(xs, gba_submaps, gba_edges2, cnct_map, total_max_iter, thread_num);
    gba_flag = 0;
}

[VERIFY: VoxelSLAM/src/voxelslam.cpp:2559-2581]

附录 A: 配置参数汇总 / Appendix A: Configuration Parameters

BTC 描述子参数 / BTC Descriptor Parameters

参数默认值高空飞行值说明
useful_corner_num_100200保留的角点数
voxel_size_1.02.0BTC体素大小(m)
plane_detection_thre_0.010.05平面检测阈值
proj_plane_num_21投影平面数
proj_image_resolution_0.50.52D投影分辨率(m)
proj_dis_max_510最大投影距离(m)
descriptor_near_num_1515KNN邻居数
descriptor_min_len_23最小三角形边长(m)
descriptor_max_len_5050最大三角形边长(m)
non_max_suppression_radius_23NMS半径(m)
std_side_resolution_0.20.2边长量化分辨率(m)

[VERIFY: VoxelSLAM/src/BTC.cpp:7-34]
[VERIFY: VoxelSLAM/src/BTC.cpp:39-66]

回环检测参数 / Loop Detection Parameters

参数配置键默认值说明
skip_near_num_(硬编码)30跳过近邻帧数
candidate_num_(硬编码)20/100候选帧数量
rough_dis_threshold_(硬编码)0.01粗匹配距离比例
similarity_threshold_(硬编码)0.7/0.5二值相似度阈值
icp_threshold_(硬编码)0.15平面ICP分数阈值
jud_defaultLoop/jud_default0.45BTC匹配分数阈值
icp_eigvalLoop/icp_eigval14ICP特征值阈值
ratio_driftLoop/ratio_drift0.05漂移比阈值
curr_haltLoop/curr_halt10自身session冷却期
prev_haltLoop/prev_halt30历史session冷却期

[VERIFY: VoxelSLAM/src/BTC.cpp:28-34]
[VERIFY: VoxelSLAM/src/voxelslam.cpp:1812-1821]

附录 B: 代码文件索引 / Appendix B: Source File Index

文件行数主要内容
VoxelSLAM/src/BTC.h1-331BTC数据结构定义、STDescManager类声明
VoxelSLAM/src/BTC.cpp1-1480BTC描述子生成/检索/验证完整实现
VoxelSLAM/src/loop_refine.hpp1-539icp_normal、add_edge、PGO_Edge/Edges、OctreeGBA
VoxelSLAM/src/voxelslam.cpp:1741-1802-build_graph 位姿图构建
VoxelSLAM/src/voxelslam.cpp:1806-2228-thd_loop_closure 回环检测主线程
VoxelSLAM/src/voxelslam.cpp:1101-1186-loop_update 回环校正传播
VoxelSLAM/src/voxelslam.cpp:1189-1228-keyframe_loading 关键帧加载
VoxelSLAM/src/voxelslam.cpp:2231-2317-topDownProcess HBA顶层优化
VoxelSLAM/src/voxelslam.cpp:2320-2482-HBA_add_edge 底层约束提取
VoxelSLAM/src/voxelslam.cpp:2485-2595-thd_globalmapping HBA建图线程

附录 C: 完整调用关系 / Appendix C: Complete Call Graph

main()
  |
  |-> thread: thd_loop_closure()
  |     |-> previous_map_read()          // 加载历史session
  |     |     |-> read_lidarstate()      // 读取位姿
  |     |     |-> GenerateSTDescs()      // 生成BTC描述子
  |     |     |-> AddSTDescs()           // 添加到数据库
  |     |
  |     |-> [主循环] for each ScanPose from buf_lba2loop:
  |           |-> 关键帧累积 (win_size scans)
  |           |-> GenerateSTDescs()       // 当前帧BTC描述子
  |           |-> SearchLoop()            // 场景检索
  |           |     |-> candidate_selector()   // 粗匹配投票
  |           |     |-> candidate_verify()     // 几何验证
  |           |           |-> triangle_solver() // SVD求解
  |           |           |-> plane_geometric_verify() // 平面ICP评分
  |           |
  |           |-> icp_normal()            // 法向量ICP精细验证
  |           |     |-> KdTree nearest neighbor
  |           |     |-> Point-to-plane least squares
  |           |     |-> Eigenvalue validation
  |           |
  |           |-> add_edge()              // 添加回环边
  |           |-> build_graph()           // 构建位姿图
  |           |-> ISAM2::update()         // GTSAM优化
  |           |-> set_state()             // 应用结果
  |           |-> loop_detect = 1         // 通知里程计线程
  |
  |-> thread: thd_globalmapping()
  |     |-> [主循环] for each Keyframe group:
  |           |-> OctreeGBA::cut_voxel()  // 体素化
  |           |-> OctreeGBA_multi_recut() // 平面提取
  |           |-> HBA_add_edge()          // 底层约束提取
  |           |     |-> Lidar_BA_Optimizer::damping_iter()
  |           |     |-> Hessian对角线提取边
  |           |
  |           |-> [系统结束时] topDownProcess()
  |                 |-> gba_edges1/2 添加到图
  |                 |-> ISAM2优化
  |                 |-> 应用结果到所有session
  |
  |-> thd_odometry_localmapping() [主线程]
        |-> if(loop_detect == 1):
              |-> loop_update()
                    |-> 清除并替换体素地图
                    |-> 应用dx变换到滑动窗口
                    |-> 重建体素地图
VSLAM(9)：回环检测
zhangzhixin1998的博客
07-28 1990
如下图所示，当前端只给出了相邻帧间的估计，由于当前帧的位姿由上一帧的位姿加上相邻帧的相对位姿决定，所以上一帧位姿的误差就延续到了当前帧。以此类推，误差就会越累计越大，使得结果跟真实轨迹越差越远，满满趋向于不稳定。后端虽然可以在一定程度上缓解误差问题，但是只有相邻帧的时候，对于累计误差也无法消除。这时候就要引入回环检测，来给出除了相邻帧外的，一些时隔更久远的约束。回环检测的目的就是识别相机是否经过同一个地方，采集到了相似的数据。如果成功检测，可以为后端提供额外的约束信息，使得最终的估计是一个。
回环检测-
qq_45488834的博客
03-29 3195
回环检测作为VSLAM中相对独立又重要的一个模块，在消除累积误差，重定位等方面有着重要作用，更有甚者，把系统是否有重定位模块，来作为判断一个系统是SLAM还是里程计的依据。
SLAM【十】回环检测
热门推荐
Cloudy_to_sunny的博客
03-18 1万+
SLAM【十】回环检测回环检测的作用及意义作用意义回环检测方法准确率和召回率词袋模型字典字典的结构功能快捷键合理的创建标题，有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注脚注释也是必不可少的KaTeX数学公式新的甘特图功能，丰富你的文章UML 图表FLowchart流程图导出与导入导出导入 回环检测的作用及意义 作用 问题：为了解决整个SLAM出现的累计误差，导致无法构建全
【视觉SLAM:九、回环检测】
计算机领域博客，与时俱进的思想与技术。
01-02 1691
全局一致性：通过检测回环，建立全局约束关系，消除里程计积累的漂移误差。地图优化：修正已有的地图，提升场景的建图精度。鲁棒性：增加对重复场景或闭环路径的适应能力。利用词袋模型，通过视觉词典对图像特征进行量化和匹配。结合相似度计算和几何验证，判断是否发生回环。通过实验分析，探索不同参数（如词典规模、关键帧选择）的影响。在实际应用中，回环检测方法需要在准确率、召回率和计算效率之间找到平衡，并根据具体场景选择合适的实现方式。
第十一讲：回环检测
qq_49561752的博客
10-28 646
回环检测、词袋模型、字典训练、相似度检验
【Voxel-SLAM】Voxel-SLAM 系统概述 / System Overview (一)
最新发布
love20102011的博客
04-28 1132
Voxel-SLAM 是一个基于 ROS 的 LiDAR-Inertial SLAM 系统，采用体素化地图(Voxel Map)作为核心数据结构，融合 IMU 预积分与自适应八叉树进行全局一致性建图。系统总代码量约 8518 行 C++，分布在两个 ROS 包中。
论文Review LIO Multi-session Voxel-SLAM | 港大MARS出品！体素+平面特征的激光SLAM！经典必读！
weixin_42148238的博客
08-04 2174
Voxel-SLAM是一种基于激光雷达和IMU的完整SLAM系统，具有以下创新点：1) 采用统一的自适应体素地图表示，覆盖初始化、里程计、局部建图、闭环和全局建图全过程；2) 提出鲁棒的初始化方法，可在动态初始状态下工作；3) 利用短期、中期、长期和多地图四种数据关联，实现高精度定位与建图；4) 引入高效的局部光束平差和全局层次优化方法。系统在室内、户外和城市等多种场景的30个数据序列上验证了其优越性能，特别是在初始化鲁棒性、多会话处理能力和退化环境重定位方面表现突出。该工作已开源，为激光SLAM领域提供了
5步精通Voxel-SLAM：从原理到实践的LiDAR惯性SLAM技术探索
gitblog_01000的博客
03-21 278
Voxel-SLAM是一套基于LiDAR惯性融合的先进SLAM系统，通过创新的体素化地图构建与多级数据关联技术，在复杂环境中实现高精度定位与建图。本文将系统解析其技术原理、部署流程、核心功能及实战应用，帮助技术探索者快速掌握这一强大工具的使用方法与优化策略。 ## 如何理解Voxel-SLAM的技术架构？ Voxel-SLAM采用模块化设计，将SLAM过程分解为协同工作的核心组件。系统架构以数
Voxel-SLAM：基于LiDAR惯性SLAM技术的环境感知与定位系统
gitblog_00930的博客
03-21 286
LiDAR惯性SLAM（同步定位与地图构建）技术是实现机器人自主导航的核心支撑，而Voxel-SLAM作为该领域的创新解决方案，通过融合激光雷达与惯性测量单元（IMU）数据，构建出高精度的三维环境地图并实现实时定位。本文将从技术原理、环境部署、功能解析、场景实践、进阶技巧到生态拓展，全面介绍Voxel-SLAM的核心价值与应用方法。 ## 技术原理：Voxel-SLAM的底层算法架构 Voxe
【视觉SLAM十四讲】第12讲 回环检测
博客标题不能为空我也没办法
12-18 3134
第12讲 回环检测
【自动驾驶100问】第八问：回环检测方法介绍
lxiao428的博客
04-14 1133
博主是《学习OpenCV4：基于Python的算法实战》和《深度学习计算机视觉实战》图书的作者，目前从事自动驾驶感知融合算法开发，分享内容包括深度学习、计算机视觉、OpenCV、自动驾驶、SLAM、C++/Python语言开发等方向的内容。《学习OpenCV4：基于Python的算法实战》：OpenCV4所有模块系统讲解，200+个实战案例，Python/C++双语编写（Python为主），对入门学习很有帮助。
最全综述 | SLAM中回环检测方法 收藏
lyk_ffl的专栏
10-09 6655
在视觉SLAM问题中，位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是我们所说的累积误差。一个消除误差有效的办法是进行回环检测。回环检测判断机器人是否回到了先前经过的位置，如果检测到回环，它会把信息传递给后端进行优化处理。回环是一个比后端更加紧凑、准确的约束，这一约束条件可以形成一个拓扑一致的轨迹地图。如果能够检测到闭环，并对其优化，就可以让结果更加准确。 在检测回环时，如果把以前的所有帧都拿过来和当前帧做匹配，匹配足够好的就是回环，但这样会导致
SLAM学习笔记-------------（11）回环检测
1900的博客
05-27 1715
回环检测的意义 我们已然介绍了前端和后端：前端提供特征点的提取和轨迹、地图的初值，而后端负责对所有这些数据进行优化。 然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得整个SLAM 出现累积误差，长期估计的结果将不可靠，或者说,我们无法构建全局一致的轨迹和地图。 虽然后端能够估计最大后验误差，但所谓“好模型架不住烂数据”，只有相邻关键帧数据时,我们能做的事情并不多，也无从消除累积误差。 但是，回环检测模块能够给出除了相邻帧的一些时隔更加久远的约束
5.0ORB-SLAM3之回环检测线程概述
超爱吃小蛋糕的66的博客
02-01 1402
ORB-SLAM3中的回环检测线程主要分为4个模块：检测共视区域、检测是否存在回环、计算相似变换、回环矫正共视区域检测主要是看关键帧和之前已经创建的关键帧之间是否存在公共单词，如果存在再进一步分析检测到的共视关键帧是其他子弟图的还是当前子地图的如果检测到的共视关键帧是其他子地图的，则将两个子地图进行融合如果检测到的共视关键帧是当前子地图的，则有可能存在回环关系如果存在回环关系则计算回环关键帧之间的相似变换，获取漂移的程度如果相似变化计算成功，则根据漂移程度纠正回环处的累计误差。
回环检测算法：Stable Trangle Descriptor
Shilong的博客
01-16 2004
回环检测是指检测传感器的两次测量（如图像、激光雷达扫描）是否发生在同一场景，它是对于SLAM问题至关重要。STD-LCD可以高效地检测闭环，并提供可靠的闭环几何变换。此外，它可以处理小重叠回环检测的挑战情况。与其他描述符中使用的多边形相比，三角形更稳定，因为三角形的形状是给定边长（或夹角）唯一确定的。此外，三角形的形状对于刚性变换是完全不变的。与关键点附近的的局部描述符相比，三角形的形状是旋转和平移不变的。
回环检测-学习笔记
luoyihao123456的博客
06-06 688
回环检测笔记
回环检测方法
大山同学学习之路
10-15 2967
近年slam的回环检测方法
详解 | SLAM回环检测问题
小白学视觉
02-18 2125
点击上方“小白学视觉”，选择加"星标"或“置顶”重磅干货，第一时间送达本文经知乎作者fishmarch授权转载，二次转载请联系作者原文：https://zhuanlan.zhihu.com...
回环检测回环校正（一）：回环检测原理
weixin_44264994的博客
01-04 8205
回环检测回环校正（一）：词袋模型 一、回环检测的意义 摘自论文：（自己敲实在太费时费力啦） [1]徐宽. 融合IMU信息的双目视觉SLAM研究[D].哈尔滨工业大学,2018. 二、词袋模型 1、概述 摘自《SLAM十四讲》 未完… ...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值