折腾笔记[7]-使用rust进行李代数计算
摘要
使用rust的nalgebra库和sophus库进行李代数hat和vee计算.
Use the nalgebra library and sophus library of rust for Lie algebra hat and vee calculation.
关键词
rust;nalgebra;SO(3);SE(3);sophus;
关键信息
[package]
name = "exp65-rust-ziglang-slambook2"
version = "0.1.0"
edition = "2021"
[dependencies]
nalgebra = { version = "0.33.2",features = ["rand"]}
rand = "0.8.5"
wgpu = "23.0.1"
winit = "0.30.8"
eframe = "0.30.0"
egui = { version = "0.30.0", features = [
"default"
]}
egui_extras = {version = "0.30.0",features = ["default", "image"]}
three-d = {path = "./static/three-d" , features=["egui-gui"] }
three-d-asset = {version = "0.9",features = ["hdr", "http"] }
env_logger = { version = "0.11.6", default-features = false, features = [
"auto-color",
"humantime",
] }
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"
# 依赖覆盖
[patch.crates-io]
pulp = { path = "./static/pulp" }
原理简介
cargo的依赖覆盖
[https://course.rs/cargo/reference/deps-overriding.html]
依赖覆盖概述
依赖覆盖是 Rust 编程中常见的一个功能,主要用于在本地开发阶段使用尚未发布到 crates.io 的包。常见场景包括:
同时开发一个包和一个项目,后者依赖于前者,需要在项目中对前者进行测试。
依赖包的 master 分支有新代码修复了 bug,希望单独测试该分支。
即将发布包的新版本,需要对其进行集成测试。
为依赖包提交了 PR 并解决了一个重要 bug,但在等待合并期间,需要使用自己修改的版本。
测试 bugfix 版本
假设项目中使用了 uuid 依赖包,但发现了一个 bug。为了修复这个 bug,首先需要将 uuid 的源码克隆到本地,并修改项目的 Cargo.toml 文件以引入本地克隆的版本。具体步骤如下:
克隆 uuid 源码到本地。
修改 Cargo.toml 文件,添加 [patch.crates-io] 部分以引入本地克隆的版本。
运行 cargo build 进行测试,如果遇到版本兼容问题,需要调整本地克隆的 uuid 版本。
使用未发布的小版本
假设要为 uuid 包新增一个特性,并已经在本地测试过。可以通过以下方式在它发布到 crates.io 之前继续使用:
修改 Cargo.toml 文件,将 [dependencies] 中的 uuid 版本提前修改为未来的版本号。
在 [patch.crates-io] 中指定 git 仓库地址。
一旦 crates.io 上有了新版本,可以移除 patch 补丁,直接更新 [dependencies] 中的 uuid 版本。
间接使用 patch
如果项目 A 的依赖是 B 和 uuid,而 B 的依赖也是 uuid,可以通过在 A 的 Cargo.toml 文件中配置 patch,使得 A 和 B 都使用来自 GitHub 的 patch 版本。
非 crates.io 的 patch
如果要覆盖的依赖不是来自 crates.io,需要对 [patch] 部分进行修改。例如,依赖是 git 仓库,可以使用本地路径来覆盖它。
使用未发布的大版本
假设要发布一个大版本 2.0.0,可以通过以下方式使用 patch 版本:
修改 Cargo.toml 文件,将 [dependencies] 中的 uuid 版本设置为 2.0。
在 [patch.crates-io] 中指定 git 仓库地址和分支名。
通过以上方法,可以在本地开发阶段灵活使用和测试尚未发布的依赖包版本,确保项目的正常进行。
李代数与李群简介
[https://zh.wikipedia.org/wiki/李代數]
[https://raw.githubusercontent.com/bexcite/slam-14/master/《李群与李代数》讲义-李世雄编着.pdf]
李群与李代数
李群(Lie Group)和李代数(Lie Algebra)是数学中的两个紧密相关且非常重要的概念,它们在理论物理、几何学和代数学等多个领域都有广泛的应用。
李群:
- 定义:李群是一个同时具有群结构和光滑流形结构的集合。简单来说,它是一个可以在其上定义连续对称性的群。
- 性质:
- 群性质:李群满足群的四个基本性质:封闭性、结合性、单位元存在性和逆元存在性。
- 流形性质:李群是一个光滑流形,意味着它可以在局部被欧几里得空间所逼近。
- 乘法光滑:群运算(乘法和逆元)是光滑的,即它们是流形上的光滑映射。
- 例子:
- 一般线性群
:所有可逆 实矩阵的集合。 - 特殊正交群
:所有行列式为1的 正交矩阵的集合。 - 单位圆群
:复平面上所有绝对值为1的复数的集合。
李代数:
- 一般线性群
- 定义:李代数是一个向量空间,配备了一个称为李括号的二元运算,满足双线性、反对称性和雅可比恒等式。
- 性质:
- 向量空间:李代数首先是一个向量空间。
- 李括号:李括号是定义在向量空间上的一个二元运算,满足特定的性质。
- 例子:
- 三维李代数
:与特殊正交群 相关联,李括号定义为向量积。 - 一般线性李代数
:所有 实矩阵的集合,李括号定义为 。
李群与李代数的关系:
- 三维李代数
- 对应:每个李群都有一个对应的李代数,李代数可以看作是李群在单位元附近的线性化。
- 同态:李群之间的同态诱导出其李代数之间的同态。
- 指数映射:李代数中的元素可以通过指数映射转化为李群中的元素,即
,其中 是李群, 是其对应的李代数。 - 结构研究:李代数提供了研究李群结构的一种有效方法,因为李代数是线性的,而李群可能是非线性的。
应用:
- 理论物理:在量子力学、广义相对论和场论中,李群和李代数用于描述对称性和守恒定律。
- 几何学:李群用于描述连续变换群,而李代数提供了研究这些群的结构和表示的强大工具。
- 代数学:李代数是研究非交换代数结构的重要工具。
sophus.rs库简介
[https://github.com/sophus-vision/sophus-rs/]
[https://docs.rs/sophus]
sophus-rs是一个用于计算机视觉和机器人应用中的二维和三维几何的Rust库。它是Sophus C++库的衍生品,后者专注于李群(例如二维和三维中的旋转和变换)。
除了李群之外,sophus-rs还包括其他几何/数学概念,如单位向量、样条曲线、图像类、相机模型,以及非线性最小二乘优化等其他实用工具。
sophus-rs is a Rust library for 2d and 3d geometry for Computer Vision and Robotics applications. It is a spin-off of the Sophus C++ library which focuses on Lie groups (e.g. rotations and transformations in 2d and 3d).
In addition to Lie groups, sophus-rs also includes other geometric/maths concepts such unit vector, splines, image classes, camera models as well as a other utilities such as a non-linear least squares optimization.
sophus的hat和vee计算
hat函数用于将一个向量转换为对应的反对称矩阵。这在李代数中非常重要,因为李代数中的元素通常用反对称矩阵表示。
hat操作后将返回一个3x3的反对称矩阵,其形式为:
[ 0 -z y ]
[ z 0 -x ]
[ -y x 0 ]
vee函数是hat函数的逆运算,用于将一个反对称矩阵转换回对应的三维向量。这在将李代数元素转换回向量表示时非常有用。
vee操作后将返回一个三维向量,其元素是对称矩阵对角线上的元素。
// hat operator
fn hat(
omega: &<S as IsScalar<BATCH, DM, DN>>::Vector<DOF>,
) -> <S as IsScalar<BATCH, DM, DN>>::Matrix<AMBIENT, AMBIENT>
// vee operator
fn vee(
hat: &<S as IsScalar<BATCH, DM, DN>>::Matrix<AMBIENT, AMBIENT>,
) -> <S as IsScalar<BATCH, DM, DN>>::Vector<DOF>
实现
- 按照如上
pulp = { path = "./static/pulp" }
方式使用本地pulp库 - 修改pulp库并进行依赖覆盖
[patch.crates-io]
pulp-0.21.4/src/lib.rs添加``#![feature(const_ptr_write)]`
+ #![feature(const_ptr_write)]
# include <iostream>
# include <cmath>
# include <Eigen/Core>
# include <Eigen/Geometry>
# include "sophus/se3.hpp"
using namespace std;
using namespace Eigen;
/// 本程序演示sophus的基本用法
int main(int argc, char **argv) {
// 沿Z轴转90度的旋转矩阵
Matrix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
// 或者四元数
Quaterniond q(R);
Sophus::SO3d SO3_R(R); // Sophus::SO3d可以直接从旋转矩阵构造
Sophus::SO3d SO3_q(q); // 也可以通过四元数构造
// 二者是等价的
cout << "SO(3) from matrix: \n" << SO3_R.matrix() << endl;
cout << "SO(3) from quaternion: \n" << SO3_q.matrix() << endl;
cout << "they are equal" << endl;
// 使用对数映射获得它的李代数
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3.transpose() << endl;
// hat为向量到反对称矩阵
cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
// 相对的,vee为反对称到向量
cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
// 增量扰动模型的更新
Vector3d update_so3(1e-4,0,0); //假设更新量为这么多
return 0;
}
重构rust代码:
#![allow(dead_code)]
#![allow(unused_variables)]
#![allow(unused_imports)]
#![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)]
use sophus::lie::LieGroup;
use nalgebra::{
Matrix3, Vector3, UnitQuaternion,
Quaternion, Isometry3, Rotation3, UnitComplex, Rotation2, Unit,
Translation3, Perspective3, Orthographic3, Vector4, Point3, Const,
ArrayStorage, Matrix4, ViewStorage
};
use std::f64::consts::PI;
fn main() {
// 1. 从Z轴旋转90度的旋转矩阵构造SO(3)李代数
// 定义旋转矩阵
// 旋转是一种可逆的、保持原点、距离和方向的变换。在代数学家中,它通常被称为n维特殊正交群SO(n)。
// 1.1 硬编码
let rotation_matrix3 = nalgebra::Matrix3::new(0.0, -1.0, 0.0,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0);
// 1.2 现场计算
// 定义旋转轴为Z轴
let axis = Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0));
// 创建绕Z轴旋转的旋转矩阵
let rotation = nalgebra::Rotation3::from_axis_angle(&axis, std::f64::consts::PI / 2.0);
// Rotation3 转 Matrix3
let rotation_matrix = rotation.matrix();
// 尝试从旋转矩阵构造Rotation3 (SO3)
let so3_r: sophus::lie::groups::rotation3::Rotation3<f64,1,0,0> = sophus::lie::groups::rotation3::Rotation3::try_from_mat(rotation_matrix).expect("无法从旋转矩阵构造Rotation3");
println!("成功构造SO(3)的Rotation3: {:?}", so3_r);
// 2. 从四元数构造SO(3)李代数
let q = nalgebra::UnitQuaternion::from_rotation_matrix(&rotation);
// 将单位四元数转换回旋转矩阵
let rotation_matrix: nalgebra::Matrix3<f64> = q.to_rotation_matrix().into();
let so3_q: sophus::lie::groups::rotation3::Rotation3<f64,1,0,0> = sophus::lie::groups::rotation3::Rotation3::try_from_mat(rotation_matrix).expect("无法从四元数构造Rotation3");
println!("SO(3) from quaternion: \n{:?}", so3_q);
// 3. 使用对数映射获得SO(3)李代数向量
let so3 = so3_r.log();
println!("so3 = {:?}", so3);
// hat为向量到反对称矩阵,显式指定泛型参数和常量参数
let so3_hat = LieGroup::<f64, 3, 4, 3, 3, 1, 0, 0, sophus_lie::groups::rotation3::Rotation3Impl<f64,1,0,0>>::hat(&so3);
println!("so3 hat=\n{:?}", so3_hat);
// 相对的,vee为反对称到向量
println!("so3 hat vee= {:?}", LieGroup::<f64, 3, 4, 3, 3, 1, 0, 0, sophus_lie::groups::rotation3::Rotation3Impl<f64,1,0,0>>::vee(&so3_hat));
// 增量扰动模型的更新,假设更新量为这么多
let update_so3 = sophus::autodiff::nalgebra::Vector3::new(1e-4, 0.0, 0.0);
}
效果
成功构造SO(3)的Rotation3: LieGroup { params: [[0.7071067811865476, 0.0, 0.0, 0.7071067811865475]], phantom: PhantomData<sophus_lie::groups::rotation3::Rotation3Impl<f64, 1, 0, 0>> }
SO(3) from quaternion:
LieGroup { params: [[0.7071067811865476, 0.0, 0.0, 0.7071067811865475]], phantom: PhantomData<sophus_lie::groups::rotation3::Rotation3Impl<f64, 1, 0, 0>> }
so3 = [[0.0, 0.0, 1.5707963267948963]]
so3 hat=
[[0.0, 1.5707963267948963, -0.0], [-1.5707963267948963, 0.0, 0.0], [0.0, -0.0, 0.0]]
so3 hat vee= [[0.0, 0.0, 1.5707963267948963]]
分析
总结性解决方案
- 四元数验证
- 给定四元数:([0.7071067811865476, 0.0, 0.0, 0.7071067811865475])
- 检查是否为单位四元数:
[
0.7071067811865476^2 + 0.0^2 + 0.0^2 + 0.7071067811865475^2 = 0.5 + 0 + 0 + 0.5 = 1
]
四元数为单位四元数,符合SO(3)要求。
- 四元数与旋转角度对应
- 四元数表示为([w, x, y, z] = [0.7071067811865476, 0.0, 0.0, 0.7071067811865475])
- 关系式:
[
w = \cos\left(\frac{\theta}{2}\right), \quad z = \sin\left(\frac{\theta}{2}\right)
] - 计算角度:
[
\cos\left(\frac{\theta}{2}\right) = 0.7071067811865476 \Rightarrow \frac{\theta}{2} \approx 45^\circ \Rightarrow \theta \approx 90^\circ
]
四元数表示绕Z轴旋转90度。
- so3向量验证
- 给定so3向量:([0.0, 0.0, 1.5707963267948963])
- 转换为角度:
[
1.5707963267948963 \text{ radians} = \frac{\pi}{2} \approx 90^\circ
]
so3向量表示绕Z轴旋转90度,与四元数一致。
- hat矩阵验证
- so3向量([0.0, 0.0, \frac{\pi}{2}])的hat矩阵:
[
] - 题目中hat矩阵:
[
]
两者基本一致,负零可忽略。
- so3向量([0.0, 0.0, \frac{\pi}{2}])的hat矩阵:
- vee操作验证
- hat矩阵通过vee操作得到so3向量:
[
[0.0, 0.0, 1.5707963267948963]
]
与原始so3向量一致。
结论
所有验证步骤一致,结果合理。
[
\boxed{结果是合理的。}
]
- hat矩阵通过vee操作得到so3向量:
【推荐】国内首个AI IDE,深度理解中文开发场景,立即下载体验Trae
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 全程不用写代码,我用AI程序员写了一个飞机大战
· MongoDB 8.0这个新功能碉堡了,比商业数据库还牛
· 记一次.NET内存居高不下排查解决与启示
· 白话解读 Dapr 1.15:你的「微服务管家」又秀新绝活了
· DeepSeek 开源周回顾「GitHub 热点速览」
2023-01-20 stm32笔记[3]-OpenOCD调试