SLAM14第四章_3

Sophus的基本使用方法

Sphus库支持本章主要讨论的SO(3)和SE(3),此外还有二维运动的SO(2),SE(2)以及相似变换Sim(3)的内容。它是直接在Eigen基础上开发的，不需要安装额外的库

• useSophus.cpp 说明
  • 前半部分介绍了SO(3)上的操作，后半部分则为SE(3)。我们演示了如何构造SO(3),SE(3)对象，对他们进行指数，对数映射，以及当知道更新量后，如何对李群元数进行更新。

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

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;

///本程序演示了sophus的基本用法
int main(int argc,char **argv)
{
    //沿z轴转90度的旋转矩阵
    Matrix3d R=AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
    //或者四元数
    Quaterniond q(R);
    Sophus::SO3d SO3_R(R);          //Sophus::SO3d 可以直接从旋转矩阵构造 这边矩阵还是R，只是放到了新的容器里面
    Sophus::SO3d SO3_q(q);          //也可以通过四元数构造

    //二者是等价的
    cout<<"SO(3) from matrix:\n" <<SO3_R.matrix()<<endl;
    cout<<"SO(3) from quaternion:\n"<<SO3_q.matrix()<<endl;
    cout<<"they are equal"<<endl;

    //使用对数映射获得他的李代数
    Vector3d so3=SO3_R.log();
    cout<<"so3= "<<so3.transpose()<<endl;
    //hat 为向量到反对称矩阵
    cout<<"so3 hang=\n"<<Sophus::SO3d::hat(so3)<<endl;
    //相对的，vee 为反对称到向量
    cout<<"so3 hat vee= "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;

    //增量扰动模型的更新
    Vector3d update_so3(1e-4,0,0);  //这就是小的更新量
    Sophus::SO3d SO3_updated=Sophus::SO3d::exp(update_so3)*SO3_R;
    cout<<"SO3 updated =\n " <<SO3_updated.matrix()<<endl;

    cout<< "*************************************"<<endl;
    // 对SE(3)操作大同小异
    Vector3d t(1,0,0);              //沿X轴平移1
    Sophus::SE3d SE3_Rt(R,t);          //从R，t构造SE(3)
    Sophus::SE3d SE3_qt(q,t);          //从q,t构造SE(3)
    cout<<"SE3 from R,t= \n" <<SE3_Rt.matrix()<<endl;
    cout<<"SE3 from q,t= \n" <<SE3_qt.matrix()<<endl;
    //李代数Se(3) 是一个六维度向量，方便期间先typedef以下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<"se = "<<se3.transpose()<<endl;
    //观察输出，会发现在Sophus中，se(3)的平移在前面，旋转在后面
    //同样地，有hat和vee两个算符,但这边这个vee是6维度到4x4的映射
    cout<< "se3 hat= \n" <<Sophus::SE3d::hat(se3)<<endl;
    cout<<" se3 hat vee= "<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<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 << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

    return 0;
}

4.4.2 例子轨迹误差评估

• 如何计算估计轨迹$T_{esti,i}$与真实轨迹$T_{gt,i}$ T=1,2,3,4,5\cdots,N之间的误差3

1. 绝对轨迹误差(Absolute Trajectory Error，ATE)
   绝对轨迹误差:同时计算平移部分和旋转部分
   $$AT{E}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N||log(T_{gt}^{-1}{T}_{esti,i}{)}^{\vee }|{|}_2^2}$$
2. 绝对平移误差(Average Translational Error)
   绝对平移误差:只是计算平移部分：
   $$AT{E}_{trans}=\sqrt{\frac{1}{N}\sum _{i=1}^N||trans(T_{gt}^{-1}{T}_{esti,i}{)}^{\vee }|{|}_2^2}$$
3. 相对平移轨迹误差(Relative Trajectory Error，PTE)
   相对平移轨迹误差：
   $$RP{E}_{all}=\sqrt{\frac{1}{N-\Delta t}\sum _{i=1}^{N-\Delta t}||log((T_{gt,i}^{-1}{T}_{gt,i+\Delta t}{)}^{-1}(T_{esti,i}^{-1}{T}_{esti,i+\Delta i}){)}^{\vee }|{|}_2^2}$$
4. 相对平移误差(RelativeTranslational Error)
   $$RP{E}_{trans}=\sqrt{\frac{1}{N-\Delta t}\sum _{i=1}^{N-\Delta t}||trans((T_{gt,i}^{-1}{T}_{gt,i+\Delta t}{)}^{-1}(T_{esti,i}^{-1}{T}_{esti,i+\Delta i}){)}^{\vee }|{|}_2^2}$$

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>

#include "sophus/se3.hpp"

using namespace Sophus;
using namespace std;

string groundtruth_file="./example/groundtruth.txt";
string estimated_file="./example/estimated.txt";

typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc,char **argv){
    TrajectoryType groundtruth=ReadTrajectory(groundtruth_file);
    TrajectoryType estimated=ReadTrajectory(estimated_file);

    //确保列表不为空，确保列表长度相等
    assert(!groundtruth.empty()&&!estimated.empty());  //如果满足条件就不发出警告，如果条件不满足就发出警告
    assert(groundtruth.size()==estimated_file.size());

    //compute rmse 计算轨迹绝对误差
    double rmse=0;
    for(size_t i=0;i<estimated.size();i++){
        Sophus::SE3d p1=estimated[i],p2=groundtruth[i];
        double error=(p2.inverse()*p1).log().norm();  //求误差之后，求L2范式
        rmse +=error*error;
    }
    rmse=rmse/double(estimated.size());
    rmse=sqrt(rmse);

    cout<<"RMSE = "<<rmse<<endl;

    DrawTrajectory(groundtruth,estimated);  //画出所有轨迹
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
// create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
/**
* glEnable(GL_DEPTH_TEST)：
*  用来开启更新深度缓冲区的功能，也就是，如果通过比较后深度值发生变化了，会进行更新深度缓冲区的操作。
* 启动它，OpenGL就可以跟踪再Z轴上的像素，这样，它只会再那个像素前方没有东西时，才会绘画这个像素。
* 在做绘画3D时，这个功能最好启动，视觉效果比较真实。
* */
  glEnable(GL_DEPTH_TEST);
/**
* Blend 混合是将源色和目标色以某种方式混合生成特效的技术。混合常用来绘制透明或半透明的物体。
* 在混合中起关键作用的α值实际上是将源色和目标色按给定比率进行混合，以达到不同程度的透明。
* α值为0则完全透明，α值为1则完全不透明。混合操作只能在RGBA模式下进行，颜色索引模式下无法指定α值。
* 物体的绘制顺序会影响到OpenGL的混合处理。*/
  glEnable(GL_BLEND);
/**
* 如果设置了glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
* 则表示源颜色乘以自身的alpha 值，目标颜色乘以1.0减去源颜色的alpha值，这样一来，
* 源颜色的alpha值越大，则产生的新颜色中源颜色所占比例就越大，而目标颜色所占比例则减小。
* 这种情况下，我们可以简单的将源颜色的alpha值理解为“不透明度”。这也是混合时最常用的方式。*/
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

//定义投影和初始模型视图矩阵
/**
* ProjectionMatrix()
* 设置相机内参，以及能看到的最大最小距离
* 如ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000)
* 表示相机分辨率，焦距，相机光心，最小最大距离*/

/**ModelViewLookAt()
* 设置观看视角，pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));
* 的意思是在世界坐标(0，-0.1，-1.8)处观看坐标原点（0,0,0）并设置Y轴向上*/
  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)
  );

//SetBounds 边界设置，最后一个是长宽比 -1024.0f/768.0f
//前四个参数表示视图在视窗中的位置(这么写是在屏幕中央)
  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 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);  //设置线宽度
    //将groundtrue 的每个点连接起来
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    //将estimated的每个点连接起来
    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      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);   // sleep 5 ms
  }
}

TrajectoryType ReadTrajectory(const string &path){
    ifstream fin(path);
    TrajectoryType trajectory;
    if (!fin){
        cerr<<"trajectory  "<<path<<" not found" <<endl;
        return trajectory;
    }

    while(!fin.eof()){
        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;
}

4.5相似变换群与李代数

相似变换群Sim(3),是指可以变换大小的一部分内容。单目相机就往往需要依靠相似变换矩阵来描述，这是由于单目相机的尺度不确定性。如下所示：


我们通过指数映射，我们能获取李代数与李群的关系，对于李代数$\zeta$.它与李群的代数关系如下：

$$s=e^{\sigma },R=exp({\varphi }^{\wedge }),t=J_s\rho$$

这里的q的公式如下：
$$q=SRp+t$$

补充

$$R{p}^{\wedge }{R}^T=(RP{)}^{\wedge }$$

$$Rexp(p^{\wedge })R^T=exp((Rp{)}^{\wedge })$$
以上公式成为SO(3)上的伴随矩阵，同样的在SE(3)上也有伴随性质：
$$Texp({\xi }^{\wedge })T^{-1}=exp((Ad(T)\xi {)}^{\wedge }))$$
其中:
$$Ad(T)=\left[\begin{array}{cc}R& t^{\wedge }R\\ 0& R\end{array}\right]$$