折腾笔记[13]-使用rust对图像建立词袋

摘要

使用rust对图像建立词袋并进行相似度评分.
Use Rust to build a bag of words for images and perform similarity scoring.

关键词

rust;slam;bag of words;DBoW3;

关键信息

项目地址:[https://github.com/ByeIO/slambook2.rs]

[package]
name = "exp65-rust-ziglang-slambook2"
version = "0.1.0"
edition = "2021"

[dependencies]
env_logger = { version = "0.11.6", default-features = false, features = [
    "auto-color",
    "humantime",
] }

# 随机数
rand = "0.8.5"
rand_distr = "0.4.3"
fastrand = "2.3.0"

# 线性代数
nalgebra = { version = "0.33.2",features = ["rand"]}
ndarray = "0.16.1"

# winit
wgpu = "23.0.1"
winit = "0.30.8"

# egui
eframe = "0.30.0"
egui = { version = "0.30.0", features = [
    "default"
]}
egui_extras = {version = "0.30.0",features = ["default", "image"]}

# three_d
three-d = {path = "./static/three-d" , features=["egui-gui"] }
three-d-asset = {version = "0.9",features = ["hdr", "http"] }

# sophus
sophus = { version = "0.11.0" }
sophus_autodiff = { version = "0.11.0" }
sophus_geo = "0.11.0"
sophus_image = "0.11.0"
sophus_lie = "0.11.0"
sophus_opt = "0.11.0"
sophus_renderer = "0.11.0"
sophus_sensor = "0.11.0"
sophus_sim = "0.11.0"
sophus_spline = "0.11.0"
sophus_tensor = "0.11.0"
sophus_timeseries = "0.11.0"
sophus_viewer = "0.11.0"
tokio = "1.43.0"
approx = "0.5.1"
bytemuck = "1.21.0"
thingbuf = "0.1.6"

# rust-cv计算机视觉
cv = { version = "0.6.0" , features = ["default"] }
cv-core = "0.15.0"
cv-geom = "0.7.0"
cv-pinhole = "0.6.0"
akaze = "0.7.0"
eight-point = "0.8.0"
lambda-twist = "0.7.0"
image = "0.25.5"
imageproc = "0.25.0"

# 最小二乘优化
gomez = "0.5.0"

# 图优化
factrs = { version = "0.2.0", features = [] }
gs-rs = { version = "0.1.0", path = "./static/gs-rs" }

# ORB角点检测
bye_orb_rs = { version = "0.1.1", path = "./static/bye_orb_rs" }

# 词袋
bye_abow_rs = { version = "0.1.0", path = "./static/bye_abow_rs" }

# 依赖覆盖
[patch.crates-io]
pulp = { path = "./static/pulp" }
mio = { path = "./static/mio", version = "1.0.3" }

原理简介

回环检测简介

[https://www.slamtec.com/cn/News/Detail/188]
回环检测（Loop Closure Detection）是SLAM（Simultaneous Localization and Mapping）系统中的关键模块，主要用于识别机器人是否回到了之前访问过的位置。其核心思想是通过比较当前场景与历史场景的相似性来判断是否发生了回环。

回环检测，又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力。说的简单点，就是机器人在左转一下，右转一下建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。

回环检测的主要作用包括：

纠正累积误差
优化地图一致性
提高定位精度

DBoW3词袋简介

[https://github.com/rmsalinas/DBow3]

Galvez-Lopez D, Tardos J D. Real-time loop detection with bags of binary words[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. 2011:51-58.

DBoW3有两个主要的类：Vocabulary和Database。视觉词典将图像转化成视觉词袋向量，图像数据库对图像进行索引。

DBoW库介绍 DBoW3是DBoW2的增强版，这是一个开源的C++库，用于给图像特征排序，并将图像转化成视觉词袋表示。它采用层级树状结构将相近的图像特征在物理存储上聚集在一起，创建一个视觉词典。 DBoW3还生成一个图像数据库，带有顺序索引和逆序索引，可以使图像特征的检索和对比非常快。
DBoW3（Bag of Words）是一种基于词袋模型的快速图像检索库，常用于视觉SLAM中的回环检测。其核心思想是将图像特征量化为视觉单词，通过比较视觉单词的分布来判断图像的相似性。

DBoW3的主要特点包括：

高效的图像检索
支持自定义特征描述子
提供多种相似度度量方法

数学表达：
给定图像 $I$ ，其特征描述子集合为 $F = {f_{1}, f_{2}, . . ., f_{n}}$ ，通过词袋模型可将其表示为：

v = (w_{1}, w_{2}, . . ., w_{m})

其中 $w_{i}$ 表示第 $i$ 个视觉单词的权重，通常使用TF-IDF（Term Frequency-Inverse Document Frequency）计算：

w_{i} = t f_{i} \times i d f_{i}

图像之间的相似度可通过余弦相似度计算：

s i m (I_{1}, I_{2}) = \frac{v_{1} \cdot v_{2}}{‖ v_{1} ‖ ‖ v_{2} ‖}

DBoW3通过构建视觉词典树（Vocabulary Tree）来加速检索过程，使得在大规模场景下仍能保持较高的效率。

kdtree简介

[https://www.cnblogs.com/aTianTianTianLan/articles/3902963.html]
[https://leileiluoluo.com/posts/kdtree-algorithm-and-implementation.html]

Kd-Tree（K维树）是一种空间划分的数据结构，常用于高维空间中的搜索，如范围搜索和最近邻搜索。它是二进制空间划分树的一种特殊情况。

在激光雷达SLAM中，通常使用三维点云，因此Kd-Tree的维度为3。由于三维点云的数量通常较大，使用Kd-Tree进行检索可以显著减少时间消耗，确保点云的关联点寻找和配准处于实时状态。

输入：无序化的点云，维度k
输出：点云对应的Kd-Tree
算法：

初始化分割轴：对每个维度的数据进行方差计算，取最大方差的维度作为分割轴，标记为r；
确定节点：对当前数据按分割轴维度进行检索，找到中位数数据，并将其放入当前节点；
划分双支：
- 划分左支：在当前分割轴维度，所有小于中位数的值划分到左支；
- 划分右支：在当前分割轴维度，所有大于等于中位数的值划分到右支。
更新分割轴：r = (r + 1) % k；
确定子节点：
- 确定左节点：在左支的数据中进行步骤2；
- 确定右节点：在右支的数据中进行步骤2。

欧几里得距离,曼哈顿距离,SO2距离和SO3距离简介

[https://hyper.ai/cn/wiki/36708]
[https://www.elastic.co/cn/blog/understanding-ann]
欧几里得距离、曼哈顿距离、SO(2)距离和SO(3)距离是几种不同的距离度量方式，它们在不同的场景和领域中有各自的应用。下面分别进行简要介绍：
欧几里得距离
定义：在n维空间中，两点之间的欧几里得距离是它们之间直线距离的长度。对于两个点x=(x1,x2,⋯,xn)和y=(y1,y2,⋯,yn)，其欧几里得距离为d(x,y)=(x1−y1)2+(x2−y2)2+⋯+(xn−yn)2。
特点：是最符合人们直观的距离度量方式，满足距离的非负性、对称性和三角不等式等性质。
应用：广泛应用于几何、物理、工程、机器学习等领域，如计算空间中两点之间的实际距离、在K - 近邻算法中衡量样本点之间的相似度等。
曼哈顿距离
定义：在n维空间中，两点之间的曼哈顿距离是它们在各坐标轴上的绝对轴距总和。对于两个点x=(x1,x2,⋯,xn)和y=(y1,y2,⋯,yn)，其曼哈顿距离为d(x,y)=∣x1−y1∣+∣x2−y2∣+⋯+∣xn−yn∣。
特点：在网格状路径中，曼哈顿距离更能准确地反映两点之间的实际行走距离，因为它不允许对角线移动。
应用：常用于城市规划、棋盘游戏（如国际象棋中马的移动距离计算）、机器学习中的特征选择等场景，特别是在数据具有明显网格结构或各特征之间相互独立时效果较好。
SO(2)距离
定义：SO(2)表示二维空间中的特殊正交群，其元素是二维旋转矩阵。SO(2)距离通常是指两个旋转矩阵之间的距离，可以通过计算它们之间的旋转角度来衡量。对于两个旋转矩阵R1和R2，其SO(2)距离可以定义为它们之间的旋转角度θ，即d(R1,R2)=θ，其中θ可以通过计算R1R2T的迹来得到。
特点：能够准确地反映二维旋转之间的差异，满足群的性质，如封闭性、结合律、单位元和逆元等。
应用：在计算机视觉、机器人学等领域中，用于衡量二维图像或物体的旋转差异，如在图像配准、目标跟踪等任务中，通过计算SO(2)距离来确定图像或物体的旋转状态。
SO(3)距离
定义：SO(3)表示三维空间中的特殊正交群，其元素是三维旋转矩阵。SO(3)距离也是指两个旋转矩阵之间的距离，通常通过计算它们之间的旋转角度来衡量。对于两个旋转矩阵R1和R2，其SO(3)距离可以定义为它们之间的旋转角度θ，即d(R1,R2)=θ，其中θ可以通过计算R1R2T的迹来得到。
特点：能够准确地反映三维旋转之间的差异，满足群的性质，如封闭性、结合律、单位元和逆元等。SO(3)是一个三维流形，其结构比SO(2)更为复杂。
应用：在机器人学、航空航天、计算机图形学等领域中，用于衡量三维物体或坐标系的旋转差异，如在机器人手臂的姿态控制、飞行器的姿态调整、三维模型的旋转变换等任务中，通过计算SO(3)距离来确定旋转状态和制定控制策略。

实现

原始cpp代码:[https://github.com/gaoxiang12/slambook2/blob/master/ch11/loop_closure.cpp]

#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>

using namespace cv;
using namespace std;

/***************************************************
 * 本节演示了如何根据前面训练的字典计算相似性评分
 * ************************************************/
int main(int argc, char **argv) {
    // read the images and database  
    cout << "reading database" << endl;
    DBoW3::Vocabulary vocab("./vocabulary.yml.gz");
    // DBoW3::Vocabulary vocab("./vocab_larger.yml.gz");  // use large vocab if you want: 
    if (vocab.empty()) {
        cerr << "Vocabulary does not exist." << endl;
        return 1;
    }
    cout << "reading images... " << endl;
    vector<Mat> images;
    for (int i = 0; i < 10; i++) {
        string path = "./data/" + to_string(i + 1) + ".png";
        images.push_back(imread(path));
    }

    // NOTE: in this case we are comparing images with a vocabulary generated by themselves, this may lead to overfit.
    // detect ORB features
    cout << "detecting ORB features ... " << endl;
    Ptr<Feature2D> detector = ORB::create();
    vector<Mat> descriptors;
    for (Mat &image:images) {
        vector<KeyPoint> keypoints;
        Mat descriptor;
        detector->detectAndCompute(image, Mat(), keypoints, descriptor);
        descriptors.push_back(descriptor);
    }

    // we can compare the images directly or we can compare one image to a database 
    // images :
    cout << "comparing images with images " << endl;
    for (int i = 0; i < images.size(); i++) {
        DBoW3::BowVector v1;
        vocab.transform(descriptors[i], v1);
        for (int j = i; j < images.size(); j++) {
            DBoW3::BowVector v2;
            vocab.transform(descriptors[j], v2);
            double score = vocab.score(v1, v2);
            cout << "image " << i << " vs image " << j << " : " << score << endl;
        }
        cout << endl;
    }

    // or with database 
    cout << "comparing images with database " << endl;
    DBoW3::Database db(vocab, false, 0);
    for (int i = 0; i < descriptors.size(); i++)
        db.add(descriptors[i]);
    cout << "database info: " << db << endl;
    for (int i = 0; i < descriptors.size(); i++) {
        DBoW3::QueryResults ret;
        db.query(descriptors[i], ret, 4);      // max result=4
        cout << "searching for image " << i << " returns " << ret << endl << endl;
    }
    cout << "done." << endl;
}

使用rust重构:

#![allow(unused_imports)]
#![allow(dead_code)]
#![allow(unused_variables)]

use std::{
    ffi::OsStr,
    path::{Path, PathBuf},
};

use bye_abow_rs::{
    keypoint::{
        all_kps_from_dir, 
        load_img_get_kps
    }, 
    vocabulary::Vocabulary, 
    BoW, 
};

fn print_progress(progress: f32) {
    let filled = (progress * 20.0) as usize;
    let empty = 20 - filled;
    print!("\r[");
    for _ in 0..filled {
        print!("#");
    }
    for _ in 0..empty {
        print!(" ");
    }
    print!("] {:.0}%", progress * 100.0);
    std::io::Write::flush(&mut std::io::stdout()).unwrap();
}

fn main() {
    // 从png图像中提取 orb 描述符(仅支持png图片)
    let n_keypoints: usize = 100;
    let train_dir = "assets/ch11-data";
    let test_dir = "assets/ch11-data";
    let voc_path = "result/ch11-loop_closure.voc";

    // 提取训练图片的特征
    println!("提取训练图片的特征...");
    let features = all_kps_from_dir(train_dir, n_keypoints).unwrap();
    println!("检测到 {} 个 ORB 特征。", features.len());

    // 从特征创建词汇表
    println!("创建词汇表...");
    let voc = Vocabulary::create(&features, 9, 3);
    println!("\n词汇表 = {:#?}", voc);

    // 保存词汇表
    println!("保存词汇表...");
    voc.save(voc_path).unwrap();

    // 加载现有词汇表
    println!("加载词汇表...");
    let vocab = Vocabulary::load(voc_path).unwrap();
    if vocab.blocks.is_empty() {
        eprintln!("词汇表不存在。");
        return;
    }
    println!("词汇表: {:#?}", voc);

    // 读取图片
    println!("读取图片...");
    let mut images = Vec::new();
    for i in 0..10 {
        let path = format!("./assets/ch11-data/{}.png", i + 1);
        images.push(Path::new(&path).to_path_buf());
    }

    // 检测 ORB 特征
    println!("检测 ORB 特征...");
    let n_keypoints = 100;
    let mut descriptors = Vec::new();
    for image in &images {
        let mut new_feat = None;
        for k in (1..=n_keypoints).rev() {
            if let Ok(feat) = load_img_get_kps(image, k) {
                new_feat = Some(feat);
                break;
            }
        }
        if let Some(feat) = new_feat {
            descriptors.push(feat);
        }
    }

    // 比较图片与图片
    println!("比较图片与图片...");
    for i in 0..images.len() {
        let bow1 = vocab.transform_with_direct_idx(&descriptors[i]).unwrap().0;
        for j in i..images.len() {
            let bow2 = vocab.transform_with_direct_idx(&descriptors[j]).unwrap().0;
            let score = bow1.l1(&bow2);
            println!("图片 {} vs 图片 {} : {}", i, j, score);
        }
        println!();
    }

    // 比较图片与数据库
    println!("比较图片与数据库...");
    let mut db = Vec::new();
    for descriptor in &descriptors {
        db.push(vocab.transform_with_direct_idx(descriptor).unwrap().0);
    }
    println!("数据库信息: 共 {} 个条目", db.len());
    for i in 0..descriptors.len() {
        let query_bow = vocab.transform_with_direct_idx(&descriptors[i]).unwrap().0;
        let mut scores = Vec::new();
        for (j, db_bow) in db.iter().enumerate() {
            let score = query_bow.l1(db_bow);
            scores.push((score, j));
        }
        scores.sort_by(|a, b| b.0.partial_cmp(&a.0).unwrap());
        println!("搜索图片 {} 返回结果: {:?}", i, &scores[..4]);
        println!();
    }

    println!("完成。");
}

效果

提取训练图片的特征...
Extracting keypoint descriptors from "assets/ch11-data/8.png"
Extracting keypoint descriptors from "assets/ch11-data/9.png"
Extracting keypoint descriptors from "assets/ch11-data/10.png"
Extracting keypoint descriptors from "assets/ch11-data/4.png"
Extracting keypoint descriptors from "assets/ch11-data/5.png"
Extracting keypoint descriptors from "assets/ch11-data/7.png"
Extracting keypoint descriptors from "assets/ch11-data/6.png"
Extracting keypoint descriptors from "assets/ch11-data/2.png"
Extracting keypoint descriptors from "assets/ch11-data/3.png"
Extracting keypoint descriptors from "assets/ch11-data/1.png"
检测到 1000 个 ORB 特征。
创建词汇表...

词汇表 = Vocabulary {
    Word/Leaf Nodes: 506,
    Other Nodes: 82,
    Levels: 3,
    Branching Factor: 9,
    Total Training Features: 1000,
    Min Word Cluster Size: 1,
    Max Word Cluster Size: 25,
    Mean Word Cluster Size: 1,
    Median Word Cluster Size: 1,
}
保存词汇表...
加载词汇表...
词汇表: Vocabulary {
    Word/Leaf Nodes: 506,
    Other Nodes: 82,
    Levels: 3,
    Branching Factor: 9,
    Total Training Features: 1000,
    Min Word Cluster Size: 1,
    Max Word Cluster Size: 25,
    Mean Word Cluster Size: 1,
    Median Word Cluster Size: 1,
}
读取图片...
检测 ORB 特征...
比较图片与图片...
当前 block id: 7, 最大 block 数: 83
当前 block id: 60, 最大 block 数: 83
当前 block id: 1, 最大 block 数: 83
当前 block id: 23, 最大 block 数: 83
当前 block id: 6, 最大 block 数: 83
当前 block id: 58, 最大 block 数: 83
搜索图片 1 返回结果: [(1.0, 1), (0.18000054, 5), (0.18000054, 6), (0.17000055, 4)]