折腾笔记[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,\mathbb{R})$：所有可逆 $n\times n$ 实矩阵的集合。
   • 特殊正交群 $SO(n)$：所有行列式为1的 $n\times n$ 正交矩阵的集合。
   • 单位圆群 $S^1$：复平面上所有绝对值为1的复数的集合。
     李代数：
4. 定义：李代数是一个向量空间，配备了一个称为李括号的二元运算，满足双线性、反对称性和雅可比恒等式。
5. 性质：
   • 向量空间：李代数首先是一个向量空间。
   • 李括号：李括号是定义在向量空间上的一个二元运算，满足特定的性质。
6. 例子：
   • 三维李代数 $\mathfrak{so}(3)$：与特殊正交群 $SO(3)$ 相关联，李括号定义为向量积。
   • 一般线性李代数 $\mathfrak{gl}(n,\mathbb{R})$：所有 $n\times n$ 实矩阵的集合，李括号定义为 $[A,B]=AB-BA$。
     李群与李代数的关系：
7. 对应：每个李群都有一个对应的李代数，李代数可以看作是李群在单位元附近的线性化。
8. 同态：李群之间的同态诱导出其李代数之间的同态。
9. 指数映射：李代数中的元素可以通过指数映射转化为李群中的元素，即 $G=\exp (\mathfrak{g})$，其中 $G$ 是李群，$\mathfrak{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)]

3. 代码
   原始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矩阵：
     [
     $$\left[\begin{array}{ccccccc}0& -\frac{\pi }{2}& 0\frac{\pi }{2}& 0& 00& 0& 0\end{array}\right]$$
     ]
   • 题目中hat矩阵：
     [
     $$\left[\begin{array}{ccccccc}0.0& 1.5707963267948963& -0.0-1.5707963267948963& 0.0& 0.00.0& -0.0& 0.0\end{array}\right]$$
     ]
     两者基本一致，负零可忽略。
5. vee操作验证
   • hat矩阵通过vee操作得到so3向量：
     [
     [0.0, 0.0, 1.5707963267948963]
     ]
     与原始so3向量一致。
     结论
     所有验证步骤一致，结果合理。
     [
     \boxed{结果是合理的。}
     ]