折腾笔记[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. 定义:李群是一个同时具有群结构和光滑流形结构的集合。简单来说,它是一个可以在其上定义连续对称性的群。
  2. 性质
    • 群性质:李群满足群的四个基本性质:封闭性、结合性、单位元存在性和逆元存在性。
    • 流形性质:李群是一个光滑流形,意味着它可以在局部被欧几里得空间所逼近。
    • 乘法光滑:群运算(乘法和逆元)是光滑的,即它们是流形上的光滑映射。
  3. 例子
    • 一般线性群 GL(n,R):所有可逆 n×n 实矩阵的集合。
    • 特殊正交群 SO(n):所有行列式为1的 n×n 正交矩阵的集合。
    • 单位圆群 S1:复平面上所有绝对值为1的复数的集合。
      李代数
  4. 定义:李代数是一个向量空间,配备了一个称为李括号的二元运算,满足双线性、反对称性和雅可比恒等式。
  5. 性质
    • 向量空间:李代数首先是一个向量空间。
    • 李括号:李括号是定义在向量空间上的一个二元运算,满足特定的性质。
  6. 例子
    • 三维李代数 so(3):与特殊正交群 SO(3) 相关联,李括号定义为向量积。
    • 一般线性李代数 gl(n,R):所有 n×n 实矩阵的集合,李括号定义为 [A,B]=ABBA
      李群与李代数的关系
  7. 对应:每个李群都有一个对应的李代数,李代数可以看作是李群在单位元附近的线性化。
  8. 同态:李群之间的同态诱导出其李代数之间的同态。
  9. 指数映射:李代数中的元素可以通过指数映射转化为李群中的元素,即 G=exp(g),其中 G 是李群,g 是其对应的李代数。
  10. 结构研究:李代数提供了研究李群结构的一种有效方法,因为李代数是线性的,而李群可能是非线性的。
    应用
  • 理论物理:在量子力学、广义相对论和场论中,李群和李代数用于描述对称性和守恒定律。
  • 几何学:李群用于描述连续变换群,而李代数提供了研究这些群的结构和表示的强大工具。
  • 代数学:李代数是研究非交换代数结构的重要工具。

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>

实现

  1. 按照如上pulp = { path = "./static/pulp" }方式使用本地pulp库
  2. 修改pulp库并进行依赖覆盖[patch.crates-io]
    pulp-0.21.4/src/lib.rs添加``#![feature(const_ptr_write)]`
+ #![feature(const_ptr_write)]
  1. 代码
    原始cpp代码:[https://github.com/gaoxiang12/slambook2/blob/master/ch4/useSophus.cpp]
# 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]]

分析

总结性解决方案

  1. 四元数验证
    • 给定四元数:([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)要求。
  2. 四元数与旋转角度对应
    • 四元数表示为([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度。
  3. so3向量验证
    • 给定so3向量:([0.0, 0.0, 1.5707963267948963])
    • 转换为角度:
      [
      1.5707963267948963 \text{ radians} = \frac{\pi}{2} \approx 90^\circ
      ]
      so3向量表示绕Z轴旋转90度,与四元数一致。
  4. hat矩阵验证
    • so3向量([0.0, 0.0, \frac{\pi}{2}])的hat矩阵:
      [
      [0π20 π200 000]
      ]
    • 题目中hat矩阵:
      [
      [0.01.57079632679489630.0 1.57079632679489630.00.0 0.00.00.0]
      ]
      两者基本一致,负零可忽略。
  5. vee操作验证
    • hat矩阵通过vee操作得到so3向量:
      [
      [0.0, 0.0, 1.5707963267948963]
      ]
      与原始so3向量一致。
      结论
      所有验证步骤一致,结果合理。
      [
      \boxed{结果是合理的。}
      ]
posted @   qsBye  阅读(18)  评论(0编辑  收藏  举报
相关博文:
阅读排行:
· 全程不用写代码,我用AI程序员写了一个飞机大战
· MongoDB 8.0这个新功能碉堡了,比商业数据库还牛
· 记一次.NET内存居高不下排查解决与启示
· 白话解读 Dapr 1.15:你的「微服务管家」又秀新绝活了
· DeepSeek 开源周回顾「GitHub 热点速览」
历史上的今天:
2023-01-20 stm32笔记[3]-OpenOCD调试
点击右上角即可分享
微信分享提示