Sophus教程

原创于 2025-11-24 17:52:26 发布 · 198 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #优化 #sophus

李群李代数专栏收录该内容

3 篇文章

订阅专栏

Sophus详细教程

Sophus 是一个用于处理李群（Lie Groups）和李代数（Lie Algebras）的 C++ 库，特别适用于机器人学、计算机视觉、图形学等领域中的位姿估计和优化问题。它基于高效的 Eigen 库，提供了对旋转和平移操作的高效实现，支持如 SO(2)、SO(3)、SE(2)、SE(3) 等常见的李群及其对应的李代数操作。

一、安装 Sophus

克隆仓库：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff  # 选择一个稳定的版本

编译安装：

mkdir build
cd build
cmake ..
make
sudo make install  # 可选，将库安装到系统目录

配置环境变量（如果未执行 sudo make install）：
- 在 .bashrc 或 .zshrc 文件中添加 Sophus 的头文件目录和库文件目录：
```
export CPLUS_INCLUDE_PATH=/path/to/Sophus:$CPLUS_INCLUDE_PATH
export LD_LIBRARY_PATH=/path/to/Sophus/build:$LD_LIBRARY_PATH
```

二、基本使用

1. 包含头文件

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"
#include "sophus/se3.hpp"

2. SO(3) 操作（三维旋转）

构造函数：

// 从旋转矩阵构造
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
Sophus::SO3d SO3_R(R);

// 从四元数构造
Eigen::Quaterniond q(R);
Sophus::SO3d SO3_q(q);

// 从旋转向量（轴角）构造（注意：不是直接使用旋转向量的三个坐标值）
// 正确的做法是通过指数映射
Eigen::Vector3d so3 = Sophus::SO3d::log(SO3_R);  // 获取李代数（旋转向量）
Sophus::SO3d SO3_exp = Sophus::SO3d::exp(so3);    // 通过指数映射构造 SO(3)

输出：

std::cout << "SO(3) from matrix: " << SO3_R.log().transpose() << std::endl;
std::cout << "SO(3) from quaternion: " << SO3_q.log().transpose() << std::endl;

李代数与李群的转换：

// 李群到李代数（对数映射）
Eigen::Vector3d so3 = SO3_R.log();

// 李代数到李群（指数映射）
Sophus::SO3d SO3_exp = Sophus::SO3d::exp(so3);

3. SE(3) 操作（三维刚体变换）

构造函数：

// 从旋转矩阵和平移向量构造
Eigen::Vector3d t(1, 0, 0);  // 平移向量
Sophus::SE3d SE3_Rt(R, t);

// 从四元数和平移向量构造
Sophus::SE3d SE3_qt(q, t);

// 从 4x4 变换矩阵构造
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
T.block<3, 3>(0, 0) = R;
T.block<3, 1>(0, 3) = t;
Sophus::SE3d SE3_T(T);

输出：

std::cout << "SE(3) from matrix and translation: " << SE3_Rt.log().transpose() << std::endl;
std::cout << "SE(3) from quaternion and translation: " << SE3_qt.log().transpose() << std::endl;

李代数与李群的转换：

// 李群到李代数（对数映射）
Eigen::Matrix<double, 6, 1> se3 = SE3_Rt.log();

// 李代数到李群（指数映射）
Sophus::SE3d SE3_exp = Sophus::SE3d::exp(se3);

三、高级功能

1. 位姿插值

SO(3) 插值（Slerp）：

double t = 0.5;  // 插值比例
Sophus::SO3d SO3_interp = Sophus::SO3d::slerp(t, SO3_R, SO3_q);

SE(3) 插值（旋转用 Slerp，平移用 Lerp）：

Sophus::SE3d SE3_interp = Sophus::SE3d::lerp(t, SE3_Rt, SE3_qt);

2. 增量扰动模型

// SO(3) 增量扰动
Eigen::Vector3d update_so3(1e-4, 0, 0);  // 扰动量
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;

// SE(3) 增量扰动
Eigen::Matrix<double, 6, 1> update_se3;
update_se3.setZero();
update_se3(1, 0) = 1e-4;  // 对平移的第二维增加扰动
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;