【SLAM】4李群&李代数

李群: 表征旋转矩阵R或变换矩阵RT的集合
李代数: 描述旋转矩阵R或变换矩阵RT的导数关系
作用是解决矩阵求导问题, 解决微小扰动等问题

4.1.基础

• 旋转矩阵++=特殊正交群$SO(3)$ $\mathrm{SO}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}$
• 变换矩阵++=特殊欧式群$SE(3)$ $\mathrm{SE}(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{SO}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$
• 对加法不封闭,对乘法封闭${\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{SO}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{SE}(3)$      ~~~~      ${\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{SO}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{SE}(3)$

群

• 群: 一种集合+一种运算的代数结构 集合A 运算· 群G=(A,·)
  • 封闭性 $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
  • 结合律 $\forall a_1,a_2,a_3\in A,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right)$
  • 幺元 $\exists a_0\in A,\text{ s.t. }\forall a\in A,a_0\cdot a=a\cdot a_0=a$
  • 逆 $\forall a\in A,\exists a^{-1}\in A,\text{ s.t. }a\cdot a^{-1}=a_0$
  • 常见
    • 整数加法$(Z,+)$      ~~~~      不含0有理数乘法$(Q\setminus 0,\cdot )$
    • 一般线性群GL(n) nxn可逆矩阵, 对矩阵乘法成群
    • 特殊正交群SO(n) 旋转矩阵群 SO(2) SO(3)
    • 特殊欧氏群SE(n) 欧氏变换 SE(2) SE(3)
  • 李群: 连续(光滑)性质的群 SO(n) SE(n) 能连续运动

李代数的引出

• 李代数

  • 任意旋转矩阵R满足 $R{R}^T=I$
  • 反对称换算${\mathbf{a}}^{\wedge }=\mathbf{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{A}}^{\vee }=\mathbf{a}$
  • 等式两边求导得反对称矩阵$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}=-{\left(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}\right)}^{\mathrm{T}}=\varphi (t{)}^{\wedge }$
  • 每对旋转矩阵求一次导, 左乘一个${\varphi }^{\wedge }(t)$矩阵, $\dot{R}(t)={\varphi }_0^{\wedge }R(t)$
    • 解得$\text{R}(t)=\exp ({\varphi }_0^{\wedge }t)$ 说明 $t=0$附近, 旋转矩阵可由$\exp ({\varphi }_0\stackrel{ˆ}{}t)$计算
  1. 给定某时刻$R$, 求得一个$\varphi$, 描述了$R$在局部的导数关系: $\varphi$正是对应到$SO(3)$上的李代数$\mathfrak{so}(3)$
  2. 给定向量$\varphi$计算矩阵指数$\exp ({\varphi }^{\wedge })$; 给定$R$计算$\varphi$: 指数/对数映射

李代数

• 一个集合V, 一个数域F, 一个二元运算[,]组成, 满足以下性质则(V,F,[,])为一个李代数
• 性质
  • 封闭性 $\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}$
  • 双线性 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F}$ 有 $\{\begin{array}{c}[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}]\\ [\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]\end{array}$
  • 自反性 $\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0$
  • 雅可比等价 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$
• 李括号[,]
  • 表达了两个元素的差异
  • 三维向量$R^3$定义的叉积是一种李括号, $g=(R^3,R,\mathrm{x})$构成一个李代数

李代数$\mathfrak{so}(3)$

• SO(3)对应的李代数是定义于$R^3$上的向量, 记作$\varphi$, 每个$\varphi$可生成一个反对称阵 $\Phi ={\varphi }^{\wedge }$
  • 李括号 $[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$
  • s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3)=\{\phi\in R^3,\Phi=\phi^{\wedge}\in R^{3\times3}\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
    • 由三维向量组成的集合, 每个向量对应一个反对称阵, 以表达旋转矩阵的导数
    • 与$SO(3)$的关系 $R=\exp ({\varphi }^{\wedge })$

李代数$\mathfrak{se}(3)$

• 李代数 $\mathfrak{se}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{l}\mathbf{\rho }\\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\varphi \in \mathfrak{so}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$
  • 六维向量$\xi$, 平移$\rho$, 旋转$\varphi$, 向量->矩阵${}^{\wedge }$, 矩阵->向量${}^{\vee }$
  • 括号 $[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$

4.2.指数与对数映射

4.2.1.$SO(3)$指数映射

• 对$\mathfrak{so}(3)$的任意元素 $\varphi$, 定义指数映射 $\exp(\phi^{\wedge)=\sum_{n=0}}{\infty}\frac{1}{n!}(\phi^\wedge)n $
  • 一顿推导得 $\exp (\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }$ (罗德里格斯公式-从旋转向量到旋转矩阵的转换)
  • 表明$\mathfrak{so}(3)$是旋转向量组成的空间
• 对数映射$\varphi =\ln (R{)}^{\vee }=({\sum }_{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }$
• 指数映射是满射
  • 每个$SO(3)$的元素可以找到一个$\mathfrak{so}(3)$元素对应
  • 可能存在多个$\mathfrak{so}(3)$元素对应同一个$SO(3)$
    • 对旋转角$\theta$, 多转$360^{\circ }$和没转一样
  • 指数映射即罗德里格斯公式
  • 旋转矩阵的导数可由旋转向量指定, 指导如何在旋转矩阵中进行微积分运算

4.2.2.$SE(3)$的指数映射

• $\begin{array}{rl}\exp \left({\mathbf{\xi }}^{\wedge }\right)& =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}{\left({\mathbf{\varphi }}^{\wedge }\right)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}{\left({\mathbf{\varphi }}^{\wedge }\right)}^n\mathbf{\rho }\\ 0^{\mathrm{T}}& \end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\mathbf{\rho }\\ 0^{\mathrm{T}}& 1\end{array}\right]=\mathbf{T}.\end{array}$
  • 左上角的R是$SO(3)$的元素, 与$\mathfrak{se}(3)$的旋转部分$\varphi$对应
  • 右上角$\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$
    • 平移部分经指数映射后, 发生了一次以$J$为系数矩阵的线性变换
    • 左上角的$R$计算旋转向量, 右上角的$t$满足$t=J\rho$

4.3.李代数求导与扰动模型

4.3.1.BCH公式&近似形式

• $\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots$
  • 两个矩阵指数积, 会产生一些由李括号组成的余项
• $SO(3)$
  • 旋转$R$-李代数$\varphi$, 左乘一个微小旋转$\Delta R$-李代数$\Delta \varphi$, 则李群上得$\Delta R\cdot R$, 李代数$J_l^{-1}(\varphi )\Delta \varphi +\varphi$
    • $\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)$
  • 李代数加法, $\varphi +\Delta \varphi$, 近似为李群上带左右雅可比的乘法
    • $\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })$
  • 此为之后做微积分的理论基础
• $SE(3)$
  • $\begin{array}{l}\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_l^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\\ \exp \left({\mathbf{\xi }}^{\wedge }\right)\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\end{array}$

4.3.2.李代数求导

• 小萝卜位姿T, 观察到世界坐标位于p的点 $z=Tp+w$
  • 观测与实际数据误差 $e=z-Tp$
  • 寻找一个最优T使整体误差最小化 ${\min }_{T}J(T)={\sum }_{i=1}^N||z_i-T{p}_i|{|}_2^2$
• 构建与位姿有关函数, 讨论该函数关于位姿的导数, 调整当前估计值
  • 李代数表示姿态, 据李代数加法对李代数求导
  • 对李群左乘/右乘微小扰动, 对扰动求导称左扰动/右扰动模型

4.3.4.扰动模型(左乘)

• 对$R$进行一次扰动$\Delta R$(李代数$\phi$), 对$\phi$求导
  • $\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$

4.3.5.$SE(3)$李代数求导

• $$\begin{gathered} \frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }} \\ \begin{array}{l}={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \mathbf{\xi }\to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0^{\mathrm{T}}\end{array}\right]}{[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^{\mathrm{T}}}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\stackrel{\text{ def }}{=}(\mathbf{T}\mathbf{p}{)}^{\odot }.\end{array} \end{gathered}$$
  • 矩阵求导$\frac{\mathrm{d}\left[\begin{array}{l}\mathbf{a}\\ \mathbf{b}\end{array}\right]}{\mathrm{d}\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]}={\left(\frac{\mathrm{d}[\mathbf{a},\mathbf{b}{]}^{\mathrm{T}}}{\mathrm{d}\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]}\right)}^{\mathrm{T}}={\left[\begin{array}{ll}\frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{x}}\\ \frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{y}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{y}}\end{array}\right]}^{\mathrm{T}}=\left[\begin{array}{cc}\frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{a}}{\mathrm{d}\mathbf{y}}\\ \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{x}}& \frac{\mathrm{d}\mathbf{b}}{\mathrm{d}\mathbf{y}}\end{array}\right]$

4.5.相似变换

• 单目视觉下存在尺度不确定性
  • 若用$SE(3)$表示位姿, 由于尺度不确定性与尺度漂移, 整个SLAM过程的尺度会发生变化
  • 显式地表达尺度因子, 对空间点p, 相机坐标系下经过一个相似变换, 而非欧氏变换
    • ${\mathbf{p}}^{\prime }=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\mathbf{p}=s\mathbf{R}\mathbf{p}+\mathbf{t}$
      • 尺度s同时作用在p的三个坐标上, 对其进行缩放
    • 相似变换群 $\mathrm{Sim}(3)=\left\{\mathbf{S}=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$
    • 李代数

实现

main.cpp

#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include "sophus/se3.hpp"
#include <pangolin/pangolin.h>
#include <unistd.h>
using namespace std;
using namespace Eigen;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType; 
typedef vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> PosesType;
void Change_SO3();
void Change_SE3();
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses);
void test_play(string TrajectFile);
void test_play2(string TrajectFile1, string TrajectFile2);
TrajectoryType ReadTrajectory(const string &path);
string estimated_file = "../estimated.txt";   //  在build文件的上一级寻找
string groundtruth_file = "../groundtruth.txt";   //  在build文件的上一级寻找
int main(int argc, char **argv)
{
    // Change_SO3();
    // Change_SE3();
    // test_play(estimated_file);
    test_play2(groundtruth_file,estimated_file);

    TrajectoryType ExtimateData = ReadTrajectory(estimated_file);
    TrajectoryType GroundData = ReadTrajectory(groundtruth_file);
    assert(!ExtimateData.empty() && !GroundData.empty());
    assert(ExtimateData.size() == GroundData.size());
    double rmse = 0;
    for(size_t i=0;i<ExtimateData.size();i++)
    {
        Sophus::SE3d p1 = ExtimateData[i], p2 = GroundData[i];
        double error = (p2.inverse() * p1).log().norm();
        rmse += error * error;
    }
    rmse = rmse / double(ExtimateData.size());
    rmse = sqrt(rmse);
    cout << "\nRMSE = " << rmse << endl;

    return 0;
}

void Change_SO3()
{
    // 沿y轴旋转90度的矩阵
    Matrix3d R = AngleAxisd(M_PI_2, Vector3d(0,1,0)).toRotationMatrix();
    Sophus::SO3d SO3_R(R);
    cout << "\nSO(3) from matrix:\n" << SO3_R.matrix() << endl;
    // 四元数创建也一样
    Quaterniond q(R);
    Sophus::SO3d SO3_q(q);
    cout << "\nSO(3) from quaternion:\n" << SO3_q.matrix() << endl;

    // 对数映射得李代数
    Vector3d so3 = SO3_R.log();
    cout << "\nso3:\n" << so3.transpose() << endl;
    // hat向量->反对称阵
    Matrix3d so3hat = Sophus::SO3d::hat(so3);
    cout << "\nso3hat:\n" << so3hat << endl;
    // vee反对称阵->向量
    Vector3d so3vee = Sophus::SO3d::vee(so3hat).transpose();
    cout << "\nso3vee:\n" << so3vee << endl;

    // 增量扰动模型更新
    Vector3d update_so3(1e-4,0,0);
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
    cout << "\nSO(3) updated:\n" << SO3_updated.matrix() << endl;
}
void Change_SE3()
{
    // 沿y轴旋转90度的矩阵
    Matrix3d R = AngleAxisd(M_PI_2, Vector3d(0,1,0)).toRotationMatrix();
    Quaterniond q(R);
    Vector3d t(1,0,0);  //  沿x轴平移1
    Sophus::SE3d SE3_Rt(R,t);   //  从Rt构造SE(3)
    cout << "\nSE3 from R,t:\n" << SE3_Rt.matrix() << endl;
    Sophus::SE3d SE3_qt(q,t);   //  从qt构造SE(3)
    cout << "\nSE3 from q,t:\n" << SE3_qt.matrix() << endl;

    typedef Eigen::Matrix<double,6,1> Vector6d; //  李代数是一个六维向量
    typedef Eigen::Matrix<double,4,4> Matrix6d; //  李代数是一个六维向量
    Vector6d se3 = SE3_Rt.log();    //  取李代数
    cout << "\nse3:\n" << se3.transpose() << endl;
    Matrix6d se3hat = Sophus::SE3d::hat(se3);
    cout << "\nse3 hat\n" << se3hat << endl;
    Vector6d se3vee = Sophus::SE3d::vee(se3hat).transpose();
    cout << "\nse3 vee\n" << se3vee << endl;

    // 增量扰动模型更新
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout << "\nSE3 update\n" << SE3_updated.matrix() << endl;
}

void DrawTrajectory(PosesType poses)
{
    pangolin::CreateWindowAndBind("Viewer",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);
        for(size_t i=0; i<poses.size(); i++)
        {
            Vector3d Ow = poses[i].translation();
            Vector3d Xw = poses[i] * (0.1 * Vector3d(1,0,0));
            Vector3d Yw = poses[i] * (0.1 * Vector3d(0,1,0));
            Vector3d Zw = poses[i] * (0.1 * Vector3d(0,0,1));
            glBegin(GL_LINES);
            glColor3f(1.0,0.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Xw[0],Xw[1],Xw[2]);
            glColor3f(0.0,1.0,0.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Yw[0],Yw[1],Yw[2]);
            glColor3f(0.0,0.0,1.0);
            glVertex3d(Ow[0],Ow[1],Ow[2]);
            glVertex3d(Zw[0],Zw[1],Zw[2]);
            glEnd();
        }
        for(size_t i=0; i<poses.size(); ++i)    //  源程序多一条黑线，可能是读取范围超出了点集
        {
            glColor3f(0.0,0.0,0.0);
            glBegin(GL_LINES);
            auto p1 = poses[i-1], p2 = poses[i];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);
    }
}

void test_play(string TrajectFile)
{
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(TrajectFile);  //  读取坐标文件
    if(!fin){
        cout << "fail to open";
    }
    while(!fin.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses.size() << "poses" << endl;
    DrawTrajectory(poses);
}

void DrawTrajectory2(PosesType poses1, PosesType poses2)
{
    pangolin::CreateWindowAndBind("Viewer",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);

        for(size_t i=0; i<poses1.size(); ++i)    //  源程序多一条黑线，可能是读取范围超出了点集
        {
            glColor3f(0.0,127.0,0.0);
            glBegin(GL_LINES);
            auto p1 = poses1[i-1], p2 = poses1[i];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for(size_t i=0; i<poses2.size(); ++i)    //  源程序多一条黑线，可能是读取范围超出了点集
        {
            glColor3f(127.0,0.0,0.0);
            glBegin(GL_LINES);
            auto p3 = poses2[i-1], p4 = poses2[i];
            glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
            glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);
    }
}

void test_play2(string TrajectFile1, string TrajectFile2)
{
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses1;
    ifstream fin1(TrajectFile1);  //  读取坐标文件
    if(!fin1){
        cout << "fail to open";
    }
    while(!fin1.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin1 >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses1.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses1.size() << "poses" << endl;

    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses2;
    ifstream fin2(TrajectFile2);  //  读取坐标文件
    if(!fin2){
        cout << "fail to open";
    }
    while(!fin2.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin2 >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses2.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses2.size() << "poses" << endl;

    DrawTrajectory2(poses1, poses2);
}
TrajectoryType ReadTrajectory(const string &path)
{
    TrajectoryType trajectory;
    ifstream fin(path);  //  读取坐标文件
    if(!fin){
        cout << "fail to open";
    }
    while(!fin.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));
        trajectory.push_back(p1);   //  放到后边
    }
    return trajectory;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(test_lie)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
add_executable(test_lie main.cpp)
target_link_libraries(test_lie Sophus::Sophus)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_LIBRARIES})
target_link_libraries(test_lie ${Pangolin_LIBRARIES})