折腾笔记[12]-使用rust进行位姿图优化

摘要

使用rust进行g2o格式的位姿图优化.
Using Rust for Pose Graph Optimization in g2o Format.

关键词

rust;g2o;slam;pose graph;optimization;

关键信息

项目地址:[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 = "0.2.0"
gs-rs = { version = "0.1.0", path = "./static/gs-rs" }

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

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

原理简介

1. 非线性最小二乘优化

• 问题描述：SLAM中的位姿图优化通常可以表示为一个非线性最小二乘问题。目标是通过最小化误差函数来优化机器人位姿和地图点的估计值。
• 数学形式：本文中给出了一个典型的非线性最小二乘优化问题的数学表达式：
  $${\mathrm{\Theta }}^{\ast }={\text{argmin}}_{\mathrm{\Theta }}\sum _i{\rho }_i(||r_i(\mathrm{\Theta })|{|}_{{\mathrm{\Sigma }}_i})$$
  其中，$\mathrm{\Theta }$ 是待优化的变量（如位姿和地图点），$r_i(\mathrm{\Theta })$ 是残差函数，${\rho }_i$ 是鲁棒核函数，${\mathrm{\Sigma }}_i$ 是协方差矩阵。

2. 因子图（Factor Graph）

• 因子图结构：因子图是一种用于表示优化问题的图结构，其中节点表示变量（如机器人的位姿和地图点），边表示约束（如观测或运动模型）。
• 变量与因子：
  • 变量（Variables）：表示待优化的未知量，通常是李群（如$SO2$、$SO3$、$SE2$、$SE3$）中的元素。
  • 因子（Factors）：表示概率约束，通常对应于观测或运动模型。每个因子都会贡献一个残差项到优化问题中。

3. 优化算法

• Gauss-Newton 优化器：一种常用的非线性最小二乘优化算法，通过迭代线性化残差函数来逼近最优解。它依赖于Hessian矩阵的正定性，如果Hessian矩阵不是正定的，优化可能会失败。
• Levenberg-Marquardt 优化器：Gauss-Newton的改进版本，通过引入阻尼因子来处理Hessian矩阵不正定的情况，增强了算法的鲁棒性。

4. 李群与李代数

• 李群（Lie Group）：在SLAM中，机器人的位姿通常表示为李群中的元素，如$SO2$（二维旋转群）、$SO3$（三维旋转群）、$SE2$（二维欧几里得群）、$SE3$（三维欧几里得群）。
• 李代数（Lie Algebra）：李代数是李群对应的切空间，优化通常在李代数中进行，因为李代数是一个向量空间，便于进行线性运算。

5. 自动微分

• 自动微分：通过使用对偶数（Dual Numbers）来实现自动微分，自动计算残差函数的导数。自动微分在优化过程中非常重要，因为它可以高效且准确地计算梯度信息。

6. 鲁棒核函数

• 鲁棒核函数：在SLAM中，观测数据可能包含噪声或异常值，鲁棒核函数（如Huber核函数）用于减少这些异常值对优化结果的影响。鲁棒核函数通过对残差进行加权，使得优化过程对异常值不敏感。

7. 位姿图优化

• 位姿图（Pose Graph）：位姿图是SLAM中的一种图结构，其中节点表示机器人的位姿，边表示位姿之间的相对运动约束。位姿图优化的目标是通过最小化位姿之间的误差来优化整个轨迹。
• g2o库：g2o是一个常用的图优化库，支持多种类型的位姿图优化。本文中提到使用(g2o库的平替)factrs库进行位姿图优化，并且使用$SE3$表示位姿。

8. Cholesky分解

• Cholesky分解：Gauss-Newton优化器依赖于Cholesky分解来求解线性系统。Cholesky分解要求矩阵是正定的，如果Hessian矩阵不是正定的，优化过程可能会失败。Levenberg-Marquardt优化器通过引入阻尼因子来处理这种情况。

实现

#![allow(dead_code)]
#![allow(unused_variables)]
#![allow(unused_imports)]
#![allow(non_fmt_panics)]
#![allow(unused_mut)]
#![allow(unused_assignments)]
#![cfg_attr(not(debug_assertions), windows_subsystem = "windows")]
#![allow(rustdoc::missing_crate_level_docs)]
#![allow(unsafe_code)]
#![allow(clippy::undocumented_unsafe_blocks)]
#![allow(unused_must_use)]
#![allow(non_snake_case)]

/*
 * 本程序演示如何用factrs solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用factrs::variables中的SE3表示位姿，它实质上是四元数而非李代数.
*/

use std::fs::File;
use std::io::{self, BufRead, Write, BufReader};
use std::path::Path;

// 线性代数
use nalgebra::{DMatrix, DVector, Matrix3, Vector3, SVector, Matrix, Vector6, Matrix6};

// 图优化
use factrs::{
    assign_symbols,
    core::{BetweenResidual, GaussNewton, Graph, Values},
    dtype, fac,
    linalg::{Const, ForwardProp, Numeric, NumericalDiff, VectorX, DiffResult, MatrixX},
    residuals::{Residual1, Residual2},
    traits::*,
    variables::{VectorVar2, SE2, VectorVar3, SE3, SO3, SO2},
    containers::Key,
    noise::{GaussianNoise},
    optimizers::{LevenMarquardt}
};

// 定义符号变量
assign_symbols!(X: SE3);

fn main() -> io::Result<()> {
    let file_path = "./assets/ch10-sphere.g2o";
    let file = File::open(file_path)?;
    let reader = io::BufReader::new(file);

    let mut graph = Graph::new();
    let mut values = Values::new();

    let mut vertex_cnt = 0;
    let mut edge_cnt = 0;

    for line in reader.lines() {
        let line = line?;
        let parts: Vec<&str> = line.split_whitespace().collect();
        if parts.is_empty() {
            continue;
        }

        match parts[0] {
            // SE3 顶点
            "VERTEX_SE3:QUAT" => {
                let index = parts[1].parse::<usize>().unwrap();
                let x = parts[2].parse::<f64>().unwrap();
                let y = parts[3].parse::<f64>().unwrap();
                let z = parts[4].parse::<f64>().unwrap();
                let qx = parts[5].parse::<f64>().unwrap();
                let qy = parts[6].parse::<f64>().unwrap();
                let qz = parts[7].parse::<f64>().unwrap();
                let qw = parts[8].parse::<f64>().unwrap();

                let rotation = SO3::from_xyzw(qx, qy, qz, qw);
                let translation = Vector3::new(x, y, z);
                let se3 = SE3::from_rot_trans(rotation, translation);
                values.insert(X(index.try_into().unwrap()), se3);
                vertex_cnt += 1;
            }
            // SE3-SE3 边
            "EDGE_SE3:QUAT" => {
                let idx1 = parts[1].parse::<usize>().unwrap();
                let idx2 = parts[2].parse::<usize>().unwrap();

                let dx = parts[3].parse::<f64>().unwrap();
                let dy = parts[4].parse::<f64>().unwrap();
                let dz = parts[5].parse::<f64>().unwrap();
                let dqx = parts[6].parse::<f64>().unwrap();
                let dqy = parts[7].parse::<f64>().unwrap();
                let dqz = parts[8].parse::<f64>().unwrap();
                let dqw = parts[9].parse::<f64>().unwrap();

                // let info_matrix = Matrix6::from_fn(|i, j| parts[10 + i * 6 + j].parse::<f64>().unwrap());

                let rotation = SO3::from_xyzw(dqx, dqy, dqz, dqw);
                let translation = Vector3::new(dx, dy, dz);
                let delta = SE3::from_rot_trans(rotation, translation);

                // 构建双变量约束关系的因子图
                let residual = BetweenResidual::new(delta);
                // let noise = GaussianNoise::<6>::from_scalar_sigma(info_matrix);
                let noise = GaussianNoise::<6>::identity();
                graph.add_factor(fac![residual, (X(idx1.try_into().unwrap()), X(idx2.try_into().unwrap())), noise]);
                edge_cnt += 1;
            }
            _ => {}
        }
    }

    println!("read total {} vertices, {} edges.", vertex_cnt, edge_cnt);

    println!("optimizing ...");
    
    /*
    当 Cholesky 分解失败时，通常是因为矩阵不是正定的。这在优化问题中可能发生，尤其是当 Hessian 矩阵（即二阶导数矩阵）不是正定时。Cholesky 分解要求矩阵必须是正定的，因此使用基于 Cholesky 分解的优化器（如 Gauss-Newton）可能会失败。
    为了解决这个问题，可以使用 Levenberg-Marquardt 优化器。该优化器通过引入阻尼因子来处理非正定的 Hessian 矩阵，因此即使在 Hessian 矩阵不是正定的情况下也能正常工作，更适合这种情况。
    */

    // let mut opt: GaussNewton = GaussNewton::new(graph.clone());
    let mut opt: LevenMarquardt = LevenMarquardt::new(graph.clone());
    
    let result = opt.optimize(values).expect("优化失败");

    println!("saving optimization results ...");

    save_g2o_file("ch10-pose_graph_g2o_SE3-result.g2o", &result, &graph).expect("保存g2o文件失败");

    Ok(())
}

fn save_g2o_file(file_path: &str, values: &Values, graph: &Graph) -> io::Result<()> {
    let mut file = File::create(file_path)?;

    // TODO: 保存顶点和保存边
    

    // FIXME: 这里直接保存整个graph没有格式化了
    writeln!(file, "{:#?}", graph)?;

    Ok(())
}

输出

read total 2500 vertices, 9799 edges.
optimizing ...
saving optimization results ...