折腾笔记[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中的位姿图优化通常可以表示为一个非线性最小二乘问题。目标是通过最小化误差函数来优化机器人位姿和地图点的估计值。
- 数学形式:本文中给出了一个典型的非线性最小二乘优化问题的数学表达式:其中,
是待优化的变量(如位姿和地图点), 是残差函数, 是鲁棒核函数, 是协方差矩阵。
2. 因子图(Factor Graph)
- 因子图结构:因子图是一种用于表示优化问题的图结构,其中节点表示变量(如机器人的位姿和地图点),边表示约束(如观测或运动模型)。
- 变量与因子:
- 变量(Variables):表示待优化的未知量,通常是李群(如
、 、 、 )中的元素。 - 因子(Factors):表示概率约束,通常对应于观测或运动模型。每个因子都会贡献一个残差项到优化问题中。
- 变量(Variables):表示待优化的未知量,通常是李群(如
3. 优化算法
- Gauss-Newton 优化器:一种常用的非线性最小二乘优化算法,通过迭代线性化残差函数来逼近最优解。它依赖于Hessian矩阵的正定性,如果Hessian矩阵不是正定的,优化可能会失败。
- Levenberg-Marquardt 优化器:Gauss-Newton的改进版本,通过引入阻尼因子来处理Hessian矩阵不正定的情况,增强了算法的鲁棒性。
4. 李群与李代数
- 李群(Lie Group):在SLAM中,机器人的位姿通常表示为李群中的元素,如
(二维旋转群)、 (三维旋转群)、 (二维欧几里得群)、 (三维欧几里得群)。 - 李代数(Lie Algebra):李代数是李群对应的切空间,优化通常在李代数中进行,因为李代数是一个向量空间,便于进行线性运算。
5. 自动微分
- 自动微分:通过使用对偶数(Dual Numbers)来实现自动微分,自动计算残差函数的导数。自动微分在优化过程中非常重要,因为它可以高效且准确地计算梯度信息。
6. 鲁棒核函数
- 鲁棒核函数:在SLAM中,观测数据可能包含噪声或异常值,鲁棒核函数(如Huber核函数)用于减少这些异常值对优化结果的影响。鲁棒核函数通过对残差进行加权,使得优化过程对异常值不敏感。
7. 位姿图优化
- 位姿图(Pose Graph):位姿图是SLAM中的一种图结构,其中节点表示机器人的位姿,边表示位姿之间的相对运动约束。位姿图优化的目标是通过最小化位姿之间的误差来优化整个轨迹。
- g2o库:g2o是一个常用的图优化库,支持多种类型的位姿图优化。本文中提到使用(g2o库的平替)factrs库进行位姿图优化,并且使用
表示位姿。
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 ...
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 全程不用写代码,我用AI程序员写了一个飞机大战
· MongoDB 8.0这个新功能碉堡了,比商业数据库还牛
· 记一次.NET内存居高不下排查解决与启示
· 白话解读 Dapr 1.15:你的「微服务管家」又秀新绝活了
· DeepSeek 开源周回顾「GitHub 热点速览」
2023-01-24 Linux运维笔记[11]-家庭局域网2
2023-01-24 OpenWrt安装Filebrowser网页文件管理器