折腾笔记[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中的位姿图优化通常可以表示为一个非线性最小二乘问题。目标是通过最小化误差函数来优化机器人位姿和地图点的估计值。
  • 数学形式:本文中给出了一个典型的非线性最小二乘优化问题的数学表达式:

    Θ=\argminΘiρi(||ri(Θ)||Σi)

    其中,Θ 是待优化的变量(如位姿和地图点),ri(Θ) 是残差函数,ρi 是鲁棒核函数,Σi 是协方差矩阵。

2. 因子图(Factor Graph)

  • 因子图结构:因子图是一种用于表示优化问题的图结构,其中节点表示变量(如机器人的位姿和地图点),边表示约束(如观测或运动模型)。
  • 变量与因子
    • 变量(Variables):表示待优化的未知量,通常是李群(如SO2SO3SE2SE3)中的元素。
    • 因子(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 ...
posted @   qsBye  阅读(6)  评论(0编辑  收藏  举报
相关博文:
阅读排行:
· 全程不用写代码,我用AI程序员写了一个飞机大战
· MongoDB 8.0这个新功能碉堡了,比商业数据库还牛
· 记一次.NET内存居高不下排查解决与启示
· 白话解读 Dapr 1.15:你的「微服务管家」又秀新绝活了
· DeepSeek 开源周回顾「GitHub 热点速览」
历史上的今天:
2023-01-24 Linux运维笔记[11]-家庭局域网2
2023-01-24 OpenWrt安装Filebrowser网页文件管理器
点击右上角即可分享
微信分享提示