Sophus库的使用

目录

1.基本用法        

2. 使用Sophus库来计算两个相机之间的运动

3.使用Sophus库进行位姿插值和路径生成

---

1.基本用法        

        Sophus是一个用于求解李群和李代数的C++库，它提供了一组基本操作和数学函数，例如构建、转换、乘法等，可以方便地进行位姿运算和优化。下面是一个使用Sophus库进行位姿运算的简单示例：

#include 
#include 
#include 

using namespace std;
using namespace Sophus;

int main() {

    // 构建旋转向量和平移向量
    Vector3d w(0.1, 0.2, 0.3);
    Vector3d v(1, 2, 3);

    // 构建SE(3)位姿
    SE3d T1(RotationMatrix3d(SO3d::exp(w)), v);
    cout << "T1 = " << T1.matrix() << endl;

    // 位姿求逆
    SE3d T2 = T1.inverse();
    cout << "T2 = " << T2.matrix() << endl;

    // 位姿相乘
    SE3d T3 = T1 * T2;
    cout << "T3 = " << T3.matrix() << endl;

    // 旋转矩阵和李代数转换
    Matrix3d R = SO3d::exp(w).matrix();
    Vector3d so3 = SO3d::log(SO3d(R));
    cout << "so3 = " << so3.transpose() << endl;

    // 李代数相加
    Vector3d w1(0.1, 0.0, 0.0);
    Vector3d w2(0.0, 0.2, 0.0);
    Vector3d w3 = w1 + w2;
    cout << "w3 = " << w3.transpose() << endl;

    return 0;
}

        在这个例子中，我们首先构建了一个SE(3)位姿T1，然后对其进行逆运算、乘法运算，最后将旋转矩阵和李代数相互转换。此外，还演示了如何将两个李代数相加。总的来说，Sophus库提供了一组简单而强大的工具，可以大大简化位姿运算和优化的过程。

---

2. 使用Sophus库来计算两个相机之间的运动

#include 
#include 
#include 
#include 
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {

  // 构造相机位姿
  // 相机1在原点，朝向z轴正方向
  Matrix3d R1 = Matrix3d::Identity();
  Vector3d t1 = Vector3d::Zero();
  Sophus::SE3d T1(R1, t1);
  cout << "camera 1 pose:\n" << T1.matrix() << endl;

  // 相机2位于(0,0,1)处，朝向与相机1相同
  Matrix3d R2 = Matrix3d::Identity();
  Vector3d t2 = Vector3d(0, 0, 1);
  Sophus::SE3d T2(R2, t2);
  cout << "camera 2 pose:\n" << T2.matrix() << endl;

  // 计算从相机1到相机2的运动
  Sophus::SE3d T12 = T2 * T1.inverse();
  cout << "camera 1 to camera 2 motion:\n" << T12.matrix() << endl;

  return 0;
}

        以上代码假设相机坐标系和世界坐标系重合，可以通过简单的修改坐标系变换来适应其他场景。

---

3.使用Sophus库进行位姿插值和路径生成

#include 
#include 
#include 
#include 
#include "se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {

  // 定义起点和终点位姿
  Vector3d t1(1, 0, 0);
  Quaterniond q1(AngleAxisd(M_PI / 4, Vector3d(0, 0, 1)));
  Sophus::SE3d pose1(q1, t1);

  Vector3d t2(0, 1, 0);
  Quaterniond q2(AngleAxisd(M_PI / 2, Vector3d(1, 0, 0)));
  Sophus::SE3d pose2(q2, t2);

  // 演示位姿插值
  for (double t = 0; t <= 1; t += 0.1) {
    Sophus::SE3d interp_pose = Sophus::SE3d::interpolate(pose1, pose2, t);
    cout << "interpolated pose at t=" << t << ":\n" << interp_pose.matrix() << endl;
  }

  // 演示路径生成
  vector path;
  for (double x = 0; x <= 1; x += 0.1) {
    for (double y = 0; y <= 1; y += 0.1) {
      Vector3d t(x, y, 0);
      Quaterniond q(AngleAxisd(M_PI / 4, Vector3d(0, 0, 1)));
      Sophus::SE3d pose(q, t);
      path.push_back(pose);
    }
  }

  // 输出路径中的位姿
  for (size_t i = 0; i < path.size(); ++i) {
    cout << "pose " << i << ":\n" << path[i].matrix() << endl;
  }

  return 0;
}

---

李群和李代数是数学中的两个重要概念，它们在很多领域中都有广泛的应用。在机器人、计算机视觉、自动驾驶、SLAM等领域中，李群和李代数也被广泛地使用。

简单来说，李群和李代数是一种用于描述旋转和平移的数学工具。其中，李群描述的是旋转和平移等连续的运动，李代数描述的是这些连续运动的切空间，即速度和角速度等。

在机器人、计算机视觉等领域中，通常需要对机器人或摄像头进行位姿估计或运动控制，这就需要对旋转和平移进行建模。使用李群和李代数可以将旋转和平移建模成一个数学上的对象，并对其进行运算和优化。

例如，对于机器人或摄像头的位姿估计，通常会使用李群和李代数描述其运动，同时也会使用李代数求解其运动方程。又如，对于自动驾驶中的路径规划和运动控制，也需要使用李群和李代数描述车辆的运动，以及对车辆的运动进行控制和优化。

总之，李群和李代数是描述旋转和平移的数学工具，在机器人、计算机视觉、自动驾驶、SLAM等领域中有广泛的应用。它们能够将旋转和平移等连续的运动建模成一个数学上的对象，并对其进行运算和优化。