Sophus的使用-17

#include <iostream>
#include <cmath>
using namespace std; 

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    
    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数
    Sophus::SO3 SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;
    
    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 为向量到反对称矩阵
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相对的，vee为反对称到向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些
    
    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated<<endl;
    
    /********************萌萌的分割线*****************************/
    cout<<"************我是分割线*************"<<endl;
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
    
    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
    
    return 0;
}

这段代码演示了 Sophus 库的使用方法。Sophus 是对 Eigen 的重要补充，专门用于处理 李群（Lie Group） 和 李代数（Lie Algebra） 的计算。

在 SLAM 算法中，我们几乎不直接对旋转矩阵 进行加法运算，而是通过 Sophus 将其转换到李代数上进行“扰动更新”。

---

1. SO(3) 部分：处理旋转

构造与表示

Sophus::SO3 SO3_R(R);          // 从 3x3 旋转矩阵构造
Sophus::SO3 SO3_v(0, 0, M_PI/2); // 从 旋转向量(0,0,pi/2) 构造
Sophus::SO3 SO3_q(q);          // 从 四元数 构造

• 核心逻辑：无论用哪种方式构造，Sophus::SO3 内部存储的都是李群（旋转状态）。
• 输出特点：当你 cout << SO3_R 时，你会发现它输出的是一个三维向量。这是因为 Sophus 默认以 **李代数 ** 的形式（即旋转向量）来显示旋转，因为这样最直观。

指数与对数映射

Eigen::Vector3d so3 = SO3_R.log(); // 对数映射：李群 -> 李代数
Sophus::SO3::exp(update_so3);      // 指数映射：李代数 -> 李群

• .log()：把旋转矩阵变成一个三维向量（旋转向量）。
• .exp()：把三维向量变回旋转矩阵。

Hat 与 Vee 算符

Sophus::SO3::hat(so3); // 向量 -> 反对称矩阵 (^)
Sophus::SO3::vee(mat); // 反对称矩阵 -> 向量 (v)

• hat：将 的向量转为 的反对称矩阵。这是李代数的标准矩阵表达形式。
• vee：它是 hat 的逆运算。

扰动更新（最重要！）

Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R;

• 这是整个李群理论在 SLAM 中的灵魂：

1. 假设我们有一个微小的扰动 update_so3。
2. 通过 exp 将其映射回李群（变成一个很小的旋转矩阵）。
3. 左乘到原旋转上，实现旋转的更新。
4. 这种方式保证了更新后的矩阵依然是正交的旋转矩阵，避免了 Eigen 直接加法导致的失效。

---

2. SE(3) 部分：处理平移 + 旋转

构造与结构

Sophus::SE3 SE3_Rt(R, t); // 使用旋转矩阵和平移向量构造

• SE3 对应的是 的变换矩阵。

李代数 的组成

typedef Eigen::Matrix<double,6,1> Vector6d;
Vector6d se3 = SE3_Rt.log();

• 六维向量：se3 是一个 6 维向量。
• 存储顺序：在 Sophus 中，前三维是 平移分量 ()，后三维是 旋转分量 ()。注意：这里的平移分量不直接等于坐标系下的 ，而是在切空间下的表达。

Hat 与 Vee 算符 (SE3版)

Sophus::SE3::hat(se3);

• 这里的 hat 会把 6 维向量转为一个 的矩阵：

---

3. 代码中的关键点总结

1. 统一性：Sophus 屏蔽了底层复杂的罗德里格斯公式和指数展开，你只需要调用 exp() 和 log()。
2. 增量更新模式：

• 矩阵做法：（错误，不满足旋转约束）。
• Sophus 做法：（正确，永远在流形上）。

3. 数据类型：代码中使用了 Vector6d，因为 SE3 的切空间是 6 维的。

---

为什么我们要学这个？

在后端的 Bundle Adjustment (BA) 优化中，我们要调整相机的位姿（旋转和平移）来让投影误差最小。
因为位姿在李群上，直接求导很难。我们通常：

1. 给位姿李代数一个微小增量 。
2. 利用 Sophus 计算这个增量对误差的影响（雅可比矩阵）。
3. 更新位姿：。

既然你已经看完了 Sophus 的基本操作，想了解一下如何利用这些工具去手动实现一个简单的位姿估计（求解雅可比矩阵）吗？