当前位置：首页 > article >正文

【C++】sophus 计算机视觉和机器人技术中的二维和三维李群 (一)

article 2025/1/11 20:57:33

Sophus

计算机视觉和机器人技术中的二维和三维李群

这是一个用于二维和三维几何问题（如计算机视觉或机器人应用）中常用的李群的C++实现。这个包包括特殊正交群SO(2)和SO(3)来表示二维和三维的旋转，以及特殊欧几里得群SE(2)和SE(3)来表示等距变换，也被称为刚体变换（即旋转和平移）在二维和三维中的应用。

状态

Sophus（又名Sophus 1）目前处于维护模式。截止2024年6月，没有计划添加新的大型功能，未来的PR可能仅限于错误修复、小改进和工具链更新。

然而，Sophus的下一代版本正在开发中：

sophus2 是Sophus的下一代C++迭代版本，是一次完全的重写。除了李群，它还包括更几何的概念，如单位向量、样条、图像类、相机模型等。

它目前作为farm-ng-core仓库的一部分进行托管，可能只有少数社区用户。尽管代码本身状况良好，但目前还没有完善的构建说明。希望不久的将来会有所改变。

sophus-rs 是Sophus的Rust版本。与sophus2类似，它包括更几何的概念，如单位向量、样条、图像类、相机模型等。此外，它还包括一个早期和实验性的非线性最小二乘优化库版本（类似于Ceres、g2o等）。

到目前为止，sophus-rs可能只有少数社区用户，但应该容易构建和实验——毕竟是用Rust编写的。

sophus-rs 的 GitHub 地址

sophus 在 crates.io 的页面

如何从源码构建 Sophus

Sophus 需要一个 C++17 编译器（尽管旧版本可以使用 C++14 构建）。

Sophus 在 Linux 和 macOS 上经过测试。它在过去也能在 Windows 上运行，不过目前没有 Windows 的持续集成（CI），因此可能需要一些小补丁才能在 Windows 上构建。

没有全面的构建说明，但查看安装脚本以及 main.yml 文件应该可以让你对如何构建所需的依赖项有一个好主意。

通过 vcpkg 安装 Sophus

你可以使用 vcpkg 依赖管理器来构建和安装 Sophus：

git clone https://github.com/Microsoft/vcpkg.gitcd vcpkg./bootstrap-vcpkg.sh./vcpkg integrate install./vcpkg install sophus

vcpkg 中的 Sophus 端口由微软团队成员和社区贡献者保持最新。如果版本过时，请在 vcpkg 仓库中创建问题或提交拉取请求。

在Sophus库中，test/core和Ceres的主要区别在于它们的用途和功能：

Sophus test/core：

用途
：Sophus的test/core主要用于测试Sophus库中的各种数学运算和Lie群（如SO3和SE3）的功能 / SE(3) with matrix storage for optimization in Ceres #185 - GitHub](https://github.com/strasdat/Sophus/issues/185)。
功能
：包含各种测试用例，确保Sophus库中的数学运算和Lie群操作的正确性和精确性 / SE(3) with matrix storage for optimization in Ceres #185 - GitHub](https://github.com/strasdat/Sophus/issues/185)。

Ceres：

用途
：Ceres是一个非线性优化库，用于求解非线性优化问题，常用于机器人、计算机视觉和其他领域的优化问题。
功能
：Ceres提供了自动微分、雅克比矩阵计算等功能，用于优化问题中的参数估计和误差最小化。

总结来说，Sophus的test/core主要用于测试库中的数学运算，而Ceres则是一个用于求解非线性优化问题的工具。

示例代码

1. hello_so3.cpp

#include <iostream> // 引入输入输出流库#include <sophus/geometry.hpp> // 引入Sophus库，用于几何计算
int main() {   // 下面演示了旋转矩阵的群乘法
  // 创建绕x、y和z轴旋转的旋转矩阵：  const double kPi = Sophus::Constants<double>::pi(); // 定义常数π  Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4); // 创建绕x轴旋转π/4的旋转矩阵R1  Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6); // 创建绕y轴旋转π/6的旋转矩阵R2  Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3); // 创建绕z轴旋转-π/3的旋转矩阵R3
  std::cout << "The rotation matrices are" << std::endl; // 输出旋转矩阵  std::cout << "R1:\n" << R1.matrix() << std::endl; // 输出R1矩阵  std::cout << "R2:\n" << R2.matrix() << std::endl; // 输出R2矩阵  std::cout << "R3:\n" << R3.matrix() << std::endl; // 输出R3矩阵  std::cout << "Their product R1*R2*R3:\n" << (R1 * R2 * R3).matrix() << std::endl; // 输出R1*R2*R3的乘积矩阵  std::cout << std::endl;
  // 旋转矩阵可以作用于向量  Eigen::Vector3d x; // 定义三维向量x  x << 0.0, 0.0, 1.0; // 初始化x向量  std::cout << "Rotation matrices can act on vectors" << std::endl; // 输出提示信息  std::cout << "x\n" << x.transpose() << std::endl; // 输出x向量  std::cout << "R2*x\n" << R2 * x << std::endl; // 输出R2作用于x后的结果  std::cout << "R1*(R2*x)\n" << (R1 * (R2 * x)).transpose() << std::endl; // 输出R1作用于R2*x后的结果  std::cout << "(R1*R2)*x\n" << ((R1 * R2) * x).transpose() << std::endl; // 输出(R1*R2)作用于x后的结果  std::cout << std::endl;
  // SO(3)群在内部表示为单位四元数  std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl; // 输出R1的矩阵形式  std::cout << "R1 in unit quaternion form:\n" << R1.unit_quaternion().coeffs().transpose() << std::endl; // 输出R1的单位四元数形式  // 注意Eigen库中四元数类的系数顺序是(imag0, imag1, imag2, real)  std::cout << std::endl;}

输出结果：

The rotation matrices areR1:        1         0         0        0  0.707107 -0.707107        0  0.707107  0.707107R2:0.866025        0      0.5       0        1        0    -0.5        0 0.866025R3:      0.5  0.866025         0-0.866025       0.5        -0       -0         0         1Their product R1*R2*R3: 0.433013      0.75       0.5-0.435596   0.65974 -0.612372-0.789149 0.0473672  0.612372
Rotation matrices can act on vectorsx0 0 1R2*x     0.5       00.866025R1*(R2*x)      0.5 -0.612372  0.612372(R1*R2)*x      0.5 -0.612372  0.612372
R1 in matrix form:        1         0         0        0  0.707107 -0.707107        0  0.707107  0.707107R1 in unit quaternion form:0.382683        0        0  0.92388

2. ensure_example.cpp

#include <iostream> // 引入输入输出流库#include <sophus/common.hpp> // 引入Sophus库中的通用模块
int main() {  SOPHUS_ENSURE(false, "There is an error. Details: {}", 42); // 使用SOPHUS_ENSURE宏进行断言，输出错误信息和详细数据}

在这段代码中，SOPHUS_ENSURE是Sophus库中的一个宏，用于在特定条件不满足时触发断言。false参数表示断言条件为假，即无条件触发断言。后面的字符串和数据用于提供详细的错误信息。在这个例子中，如果代码运行到这里，会输出 "There is an error. Details: 42" 这样的错误信息。

3. custom_ensure_handler.cpp

#include <sophus/common.hpp> // 引入Sophus库中的通用模块#include <cstdio> // 引入C标准输入输出库#include <cstdlib> // 引入C标准库
namespace Sophus { // 定义Sophus命名空间  void ensureFailed(char const* function, char const* file, int line, char const* description) {    // 定义一个名为ensureFailed的函数，该函数接受四个参数：    // function：发生错误的函数名    // file：发生错误的文件名    // line：发生错误的行号    // description：错误的描述信息
    std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", file, function, line);    // 使用printf函数输出错误信息，包括文件名、函数名和行号
    throw std::runtime_error("Sophus ensure failed");    // 抛出一个运行时错误异常，表示确保操作失败  }} // namespace Sophus