SLAM匹配之无向图------最大团搜索算法总结

Hermit_Rabbit

10624人浏览 · 2025-01-22 09:58:55

Hermit_Rabbit · 2025-01-22 09:58:55 发布

0. 概述

最大团搜索算法旨在从给定的图中找到最大的完全子图（团），即一个所有点彼此相连的子集。给定无向图G=(V,E)。如果U包含于V，且对任意u，v属于U且有(u，v)属于E，则称U是G的完全子图。G的完全子图U是G的团当且仅当U不包含在G的更大的完全子图中，即U就是最大完全子图。这里作者写了一个简单的例子：https://github.com/lovelyyoshino/bron_kerbosch

团 (Clique) 的定义与例子

在图论中，团 (Clique) 是图中一个顶点的子集，使得子集中任意两个顶点都有一条边相连。换句话说，一个团是一个完全子图。

0.1 基本概念

极大团: 一种团，无法再添加任何其他顶点而保持团的性质。这意味着如果你尝试向这个极大团中添加任何其他顶点，该顶点至少与团中的一个顶点不相连。
最大团: 在所有极大团中，顶点数量最多的团。

0.2 示例图

考虑以下无向图 G：

     A
   / | \
 B  |  C
   \ | /
     D

边集 ( E = {(A,B), (A,C), (A,D), (B,D), (C,D)} )

0.3 寻找团

在这个图中，我们可以找出几个团：

团:
- ${A, B\}$ : 是一个团。
  -$ {A, C}$: 是一个团。
- ${A, D\}$ 是一个团。
- ${B, D\}$ : 是一个团。
- ${C, D\}$ : 是一个团。
- ${A, B, D\}$ : 是一个团，因为 $A, B, D$ 之间都有边。
${A, C, D\}$ : 是一个团，因为 $A, C, D$ 之间都有边。
极大团:
- ${A, B, D\}$ : 这个团是极大的，因为我们不能再加入其他顶点（比如 $C$ ）而保持团的性质。
- ${A, C, D\}$ : 同样是一个极大团。
最大团:
- 在上述极大团中， ${A, B, D\}$ 和 ${A, C, D\}$ 都包含 3 个顶点，因此它们都是最大团。

综上所属，极大团需要两两有联系，且这个团不能再加入其他顶点而保持团的性质。

1. Bron–Kerbosch 算法

求解最大团、极大团这里用到了Bron–Kerbosch算法。Bron-Kerbosch 算法是一个经典的递归回溯算法，用于寻找图中的所有极大团。其基本思路依赖于三个集合的递归搜索：

R: 当前已找到的团。
P: 可能加入团的顶点集合（也就是说可能与R集合中所有点都有边存在的点）。
X: 已被添加到团的顶点集合（作用是判重）。

1.1 算法步骤

初始化 R 和 X 为 NULL，P 为图中所有顶点的集合。
从 P 中逐个取出顶点 v：
- 将 v 加入 R。
- 递归调用自身，更新 P 和 X。
- 从 P 中删除 v，并将 v 添加到 X。
当 P 和 X 都为空时，报告 R 为一个极大团。

1.2 优化方法

关键点 (Pivot Vertex): 通过选择一个关键点来减少搜索空间。
预排序: 在开始时对所有点进行排序，从而避免重复计算。

2. Bron–Kerbosch 的C++ 实现

实现代码使用了深度优先搜索 (DFS) 和动态规划 (DP) 技术，以提高效率。代码的结构包括：

数据结构定义，包含图的邻接矩阵、最大团大小、DP 数组和 DFS 层级数组。
init 函数用于初始化图。
addedge 函数用于添加边。
dfs 函数用于递归查找最大团并进行剪枝。
solver 函数整合整个算法逻辑，调用 DFS，计算最大团的大小。

这里我们参考无向图的极大团、最大团(Bron-Kerbosch算法)内容的代码，实现了一个高效的最大团搜索算法。

#include <cstring>
#include <iostream>
using namespace std;

constexpr int MAXN = 105;  // 最大节点数

struct MaxClique {
  bool g[MAXN][MAXN];  // 邻接矩阵，表示图的边
  int n, dp[MAXN], st[MAXN][MAXN], ans;  // n: 节点数; dp: 动态规划数组; st: 存储可能的最大团的节点集合; ans: 当前找到的最大团大小

  // 初始化图
  void init(int n) {
    this->n = n;  // 设置节点数量
    memset(g, false, sizeof(g));  // 初始化邻接矩阵为 false
  }

  // 添加边
  void addedge(int u, int v, bool w) { 
    g[u][v] = w;  // 设定节点 u 和节点 v 之间的连接
  }

  // 深度优先搜索，用于查找最大团
  bool dfs(int sz, int num) {
    if (sz == 0) {  // 如果当前层没有节点
      if (num > ans) {  // 如果找到的团大小大于当前最大团
        ans = num;  // 更新最大团大小
        return true;  // 找到更大的团
      }
      return false;  // 否则返回 false
    }
    
    for (int i = 0; i < sz; i++) {  // 在当前层的集合中枚举一个点 i
      if (sz - i + num <= ans) return false;  // 剪枝1: 如果剩余节点加上已找到的团小于等于最大团，提前返回
      int u = st[num][i];  // 获取当前层选择的节点
      if (dp[u] + num <= ans) return false;  // 剪枝2: 如果以当前节点为起始点的最大团加上当前团小于等于最大团，提前返回
      
      int cnt = 0;  // 记录下一层的有效节点数量
      for ( int j = i + 1; j < sz;  j++) {// 遍历在 i 之后的节点
        // 如果节点 u 和 st[num][j] 之间有边，加入下一层集合
        if (g[u][st[num][j]]) st[num + 1][cnt++] = st[num][j];
      }
      if (dfs(cnt, num + 1)) return true;  // 递归调用
    }
    return false;  // 如果没有找到更大的团，返回 false
  }

  // 主求解函数
  int solver() {
    ans = 0;  // 初始化最大团大小
    memset(dp, 0, sizeof(dp));  // 初始化 dp 数组为 0
    for (int i = n; i >= 1; i--) {  // 从最后一个节点向前遍历
      int cnt = 0;  // 重置当前层的节点计数
      for (int j = i + 1; j <= n; j++) {  // 初始化第 1 层集合
        if (g[i][j]) st[1][cnt++] = j;  // 如果 i 和 j 之间有边，加入集合
      }
      dfs(cnt, 1);  // 调用 DFS
      dp[i] = ans;  // 更新以第 i 个节点为起点的最大团大小
    }
    return ans;  // 返回最大团大小
  }

} maxclique;  // 创建 MaxClique 实例

int main() {
  cin.tie(nullptr)->sync_with_stdio(false);  // 提高输入输出效率
  int n;
  while (cin >> n, n) {  // 读取节点数量，直到输入为 0
    maxclique.init(n);  // 初始化图
    for (int i = 1; i <= n; i++) {
      for (int j = 1; j <= n; j++) {
        int x;  // 读取边信息
        cin >> x;  // 输入 0 或 1
        maxclique.addedge(i, j, x);  // 添加边
      }
    }
    cout << maxclique.solver() << '\n';  // 输出最大团大小
  }
  return 0;  // 程序结束
}

3. 高斯玻色采样（GBS）+ 局域搜索算法

这里是实现了结合高斯玻色采样（GBS）和局域搜索算法来寻找图的最大团。这个示例代码使用了邻接矩阵来表示图，并包含了采样、团收缩以及局域搜索的功能。

图的邻接矩阵编码：将给定图 $G$ 的邻接矩阵 $A$ 编码到高斯玻色采样线路中，生成 $N$ 个样本。
后选择处理：对生成的样本进行后选择，挑选出与稠密子图对应的样本。稠密子图的样本具有更高的采样概率。
形成团体：
- 对每个后选择的样本，重复以下步骤：
  - 计算子图中每个节点的连接数。
  - 移除连接数最少的节点，直到剩下的节点形成一个团（即所有节点之间都有边相连）。
局部搜索算法：
- 计算当前团的集合 $C_0(C)$ ，随机挑选一些节点加入这个团，形成更大的团，直到节点数不再增长。
替换操作：
- 基于步骤 4 的结果，计算新的集合 $C_1(C)$ 。随机挑选 $C_1(C)$ 中的节点替换当前团 $C$ 中不连接的节点，得到新的团。
- 将新的团作为起点回到步骤 3。
收敛检测：重复步骤 3 和 4，直到团的大小不再变化（即收敛）。
输出最大团：挑选出基于后选择样本得到的最大团。

#include <iostream>
#include <vector>
#include <unordered_map>
#include <algorithm>
#include <random>
#include <stdexcept>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

namespace boost {
    using namespace boost; // 使用 Boost 的命名空间
}

// 定义图的类型
typedef adjacency_list<vecS, vecS, undirectedS> Graph;

// 检查一个子图是否是团的函数
bool is_clique(const Graph& graph, const std::vector<int>& vertices) {
    // 检查所有顶点对是否都有边相连
    for (size_t i = 0; i < vertices.size(); ++i) {
        for (size_t j = i + 1; j < vertices.size(); ++j) {
            if (!edge(vertices[i], vertices[j], graph).second) {
                return false; // 如果有一对没有边相连，则不是团
            }
        }
    }
    return true; // 所有顶点对都有边相连，返回 true
}

// 减少团的函数（寻找小团体）
std::vector<std::vector<int>> find_smaller_cliques(const std::unordered_map<std::vector<int>, int>& samples, const Graph& graph, const std::string& selection_method = "uniform") {
    std::vector<std::vector<int>> smaller_cliques; // 存储找到的小团
    int max_size = 0; // 最大团的大小

    // 遍历所有样本
    for (const auto& sample : samples) {
        const auto& vertex_indices = sample.first; // 获取样本中的顶点索引
        std::vector<int> subgraph; // 创建子图

        // 检查子图中的每个顶点索引是否有效
        for (int idx : vertex_indices) {
            if (idx >= num_vertices(graph)) {
                throw std::invalid_argument("Invalid vertex index in the subgraph");
            }
            subgraph.push_back(idx);
        }

        Graph subgraph_copy = graph; // 复制图以便进行修改
        // 通过移除顶点来调整子图，直到它成为一个团
        while (!is_clique(subgraph_copy, subgraph)) {
            std::vector<int> degree_count(num_vertices(subgraph_copy));
            degree_map(degree_count, subgraph_copy); // 计算每个顶点的度数

            auto min_degree_it = std::min_element(degree_count.begin(), degree_count.end());
            int vertex_to_remove_index;

            // 根据指定的选择方法找到要移除的顶点
            if (selection_method == "uniform") {
                vertex_to_remove_index = std::distance(degree_count.begin(), min_degree_it);
            } else {
                throw std::invalid_argument("Unrecognized vertex selection method");
            }

            int vertex_to_remove = subgraph_copy[vertex_to_remove_index]; // 获取要移除的顶点
            remove_vertex(vertex_to_remove, subgraph_copy); // 从子图中移除该顶点
        }

        // 如果当前团的大小大于之前记录的最大值，更新结果
        if (subgraph_copy.num_vertices() > max_size) {
            max_size = subgraph_copy.num_vertices();
            smaller_cliques.push_back(subgraph_copy.get_vertices()); // 保存当前的团
        }
    }

    return smaller_cliques; // 返回找到的小团
}

// 创建随机图并填充邻接列表
Graph create_random_graph(int num_vertices, double edge_probability) {
    Graph graph; // 创建图对象
    std::default_random_engine generator; // 随机数生成器
    std::uniform_real_distribution<double> distribution(0.0, 1.0); // 均匀分布

    // 随机添加边
    for (int i = 0; i < num_vertices; ++i) {
        for (int j = i + 1; j < num_vertices; ++j) {
            if (distribution(generator) < edge_probability) {
                add_edge(i, j, graph); // 以给定的概率添加边
            }
        }
    }
    return graph; // 返回生成的图
}

// 创建样本数据
std::unordered_map<std::vector<int>, int> generate_samples(int num_samples, int sample_size, int max_vertex) {
    std::unordered_map<std::vector<int>, int> samples; // 存储样本的映射
    std::default_random_engine generator; // 随机数生成器
    std::uniform_int_distribution<int> vertex_distribution(0, max_vertex - 1); // 顶点选择分布

    // 生成样本
    for (int i = 0; i < num_samples; ++i) {
        std::vector<int> sample;
        for (int j = 0; j < sample_size; ++j) {
            int vertex = vertex_distribution(generator);
            sample.push_back(vertex); // 随机选择顶点
        }
        samples[sample] = i; // 将样本与唯一的 ID 映射
    }
    return samples; // 返回生成的样本
}

int main() {
    const int num_vertices = 10; // 图中顶点数量
    const double edge_probability = 0.3; // 边的生成概率
    const int num_samples = 5; // 生成的样本数量
    const int sample_size = 3; // 每个样本的顶点数量

    // 创建随机图
    Graph graph = create_random_graph(num_vertices, edge_probability); // 创建随机图

    // 创建样本数据
    std::unordered_map<std::vector<int>, int> samples = generate_samples(num_samples, sample_size, num_vertices); // 生成样本

    // 挑选子图并寻找最大团
    std::vector<std::vector<int>> clique_set; // 存储找到的团
    int max_len = 0; // 记录最大团的大小

    // 对每个样本进行处理
    for (const auto& sample : samples) {
        auto temp_cliques = find_smaller_cliques(sample, graph); // 找到小团
        for (const auto& small_clique : temp_cliques) {
            // TODO: 使用局部搜索算法找到最大团
            // 此处实现可以根据需求进行扩展
            if (small_clique.size() > max_len) {
                max_len = small_clique.size();
                clique_set.clear(); // 清空之前的团
                clique_set.push_back(small_clique); // 更新新的最大团
            }
        }
    }

    // 输出最大团
    for (const auto& clique : clique_set) {
        std::cout << "Clique: ";
        for (int node : clique) {
            std::cout << node << " "; // 输出团中的每个节点
        }
        std::cout << std::endl; // 换行
    }

    return 0; // 结束程序
}

图的生成：您需要实现生成图的逻辑，特别是使用 erdos_renyi 函数生成随机图。
GBS 实例和采样：需要根据 C++ 的可用库实现 GBS 和采样的相关逻辑。
最大团的搜索：您需要实现 clique.search 函数的相应逻辑。
绘图：绘图库的使用可能需要根据您系统的配置进行调整。

在这里插入图片描述

4. 几何一致性与最大团

SegMatch中提到的几何一致性。在原文说到：给定查询的分段点云，接下来的目标是识别以前生成地图中分段点云的匹配的任务。这是通过首先在特征空间中使用KD树搜索检索相关分段点云，然后将每个检索片段分类为匹配或不匹配。由于选择合适的距离度量和阈值往往比较困难，因此对随机森林进行训练，并对其是否匹配进行分类。一旦分段点云匹配通过分类器的确定，再利用分段点云质心之间的对应的关系的几何一致性检查验证匹配度。如果场景几何一致，返回6自由度姿态，在便在地图提供的定位信息。相关的代码可以参考Github。我们这里来看一下这个代码的具体思想

对于匹配的几何一致性问题，这里我们先定义了匹配的格式信息segmap-master/segmatch/include/segmatch/common.hpp

class PairwiseMatch {
 public:
 /**
 // @brief 表示一对点云之间的匹配，包括它们的ID、质心、置信度等信息
 // @param  id1              第一个点云的标识符
 // @param  id2              第二个点云的标识符
 // @param  centroid1        第一个点云的质心坐标
 // @param  centroid2        第二个点云的质心坐标
 // @param  confidence_in    匹配的置信度值
  */
  PairwiseMatch(Id id1, Id id2, const PclPoint& centroid1, const PclPoint& centroid2,
                float confidence_in) :
                  ids_(id1, id2),
                  confidence_(confidence_in),
                  centroids_(PointPair(centroid1, centroid2)) {}

  // @brief 返回当前 PairwiseMatch 对象中的质心信
  PointPair getCentroids() const { return centroids_; }
  IdPair ids_;
  float confidence_;
  Eigen::MatrixXd features1_;
  Eigen::MatrixXd features2_;
  PointPair centroids_;
};

// 定义一个新的类型 PairwiseMatches，它是 PairwiseMatch 对象的动态数组（向量），并且使用 Eigen::aligned_allocator 来确保内存对齐
typedef std::vector<PairwiseMatch,
    Eigen::aligned_allocator<PairwiseMatch> > PairwiseMatches;
    
struct Translation {
  Translation(double x_in, double y_in, double z_in) :
    x(x_in), y(y_in), z(z_in) {}
  double x;
  double y;
  double z;
};

  // @brief 测试用，返回一组不包含任何聚类的配对匹配数据
  // @return PairwiseMatches 
   */
  static PairwiseMatches getMatchesNoCluster() {
    //其中包含若干个 PairwiseMatch 实例
    return {
      PairwiseMatch(1, 11, { 2.0f, 0.0f, 0.0f }, {   5.0f, 0.0f, 0.0f }, 1.0f), // Group 1
      PairwiseMatch(1, 12, { 2.0f, 0.0f, 0.0f }, {   8.0f, 0.0f, 0.0f }, 1.0f), // Group 2
      PairwiseMatch(2, 13, { 4.0f, 0.0f, 0.0f }, {  10.0f, 0.0f, 0.0f }, 1.0f), // Group 2
      PairwiseMatch(3, 14, { 6.0f, 0.0f, 0.0f }, { 150.0f, 0.0f, 0.0f }, 1.0f)  // Group 3
    };
  }