一、Sophus的基本使用

注：在编译前，应该装好Sophus库，该库要用到一些依赖，可自行去csdn查找
在github的slambook2(by_gaoxiang)上给出了相关的第三方库

该程序主要包含简单的李群李代数的的定义以及转换
源码如下：

//
//该程序包含Sophus库的基本使用
//包括 李群（SO3、SE3）  、 李代数（so3、 se3）  以及相应的转换
#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include"sophus/se3.hpp"
using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{
    cout<<"**********************************下面是SO3、 so3(旋转) ***********************************"<<endl;
    Matrix3d R;
    AngleAxisd  rotation_vector(M_PI/2,Vector3d(0,0,1));//旋转向量，绕z轴旋转90°
    R=rotation_vector.toRotationMatrix();//得到旋转矩阵R
    cout<< "旋转矩阵 R = " <<endl<<R<<endl<<endl;
    Quaterniond q(R);//得到四元数
    cout<< "四元数 q = " <<endl<<q.coeffs()<<endl<<endl;
    cout<<" *********************************李群*************************** "<<endl;
    Sophus::SO3d SO3_R(R);//李群是矩阵
    Sophus::SO3d SO3_q(q);
    cout<<"SO3 from rotation matrix : \n"<< SO3_R.matrix() <<endl;
    cout<<"SO3 from Quaternion : \n"<< SO3_q.matrix() <<endl;
    cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
    Vector3d so3=SO3_R.log();//李代数是三维向量
    cout<<"so3 = "<< so3.transpose()<<endl;
    cout<<" *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** "<<endl;
    cout<<"so3 hat= \n"<< Sophus::SO3d::hat(so3)<<endl;
    cout<<"so3 hat vee = "<< Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;
    cout<<" *********************************增量扰动模型的更新*************************** "<<endl;
    Vector3d so3_update(1e-4,0,0);//用李代数表示更新，李代数可以实现加减
    Sophus::SO3d SO3_update =Sophus::SO3d::exp(so3_update)*SO3_R;//先用李代数得到更新量，再把更新量变为李群，左乘原来的李群就得到更新了
    cout<<"SO3 update = \n"<< SO3_update.matrix() <<endl;
    cout<<"**********************************下面是SE3、 se3(将旋转和平移放在一起，也就是整个的变换) ***********************************"<<endl;
    //和上面SO3 类似
    Vector3d t(1,0,0);//translation
    Sophus::SE3d SE3_Rt(R,t);//李群SE3_Rt由旋转矩阵R和平移t构成
    Sophus::SE3d SE3_qt(q,t);//李群SE3_Rt由四元数q和平移t构成
    cout<<"SE3 from rotation matrix and t : \n"<< SE3_Rt.matrix() <<endl;
    cout<<"SO3 from Quaternion and t : \n"<< SE3_qt.matrix()<<endl;
    cout<<" *********************************利用对数映射得到李代数*************************** "<<endl;
    //李代数se3是六维向量，前三维表示平移，后三维表示旋转
    //利用typedef定义一个Vector6d
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3=SE3_Rt.log();
    cout<<" se3 = "<<se3.transpose()<<endl;
    cout<<" *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** "<<endl;
    cout<<"se3 hat= \n"<< Sophus::SE3d::hat(se3)<<endl;
    cout<<"so3 hat vee = "<< Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
    Vector6d se3_update;
    se3_update.setZero();
    se3_update(0,0)=1e-4;//得到更新量（1e-4d,0,0,0,0,0）
    Sophus::SE3d SE3_update= Sophus::SE3d::exp(se3_update)*SE3_Rt;//先将更新量变为李群，再右乘原来的SE3，得到更新量
    cout<<"SE3 update = \n"<< SE3_update.matrix() <<endl;//得到更新后的SE3
    return 0;
}

运行结果：

**********************************下面是SO3、 so3(旋转) ***********************************
旋转矩阵 R = 
6.12323e-17          -1           0
          1 6.12323e-17           0
          0           0           1

四元数 q = 
       0
       0
0.707107
0.707107

 *********************************李群*************************** 
SO3 from rotation matrix : 
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
SO3 from Quaternion : 
2.22045e-16          -1           0
          1 2.22045e-16           0
          0           0           1
 *********************************利用对数映射得到李代数*************************** 
so3 =      0      0 1.5708
 *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** 
so3 hat= 
      0 -1.5708       0
 1.5708       0      -0
     -0       0       0
so3 hat vee =      0      0 1.5708
 *********************************增量扰动模型的更新*************************** 
SO3 update = 
          0          -1           0
          1           0     -0.0001
     0.0001 2.03288e-20           1
**********************************下面是SE3、 se3(将旋转和平移放在一起，也就是整个的变换) ***********************************
SE3 from rotation matrix and t : 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
SO3 from Quaternion and t : 
2.22045e-16          -1           0           1
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1
 *********************************利用对数映射得到李代数*************************** 
 se3 =  0.785398 -0.785398         0         0         0    1.5708
 *****利用hat()函数，可以将向量变到反对称矩阵，利用vee()函数，可以将反对称矩阵变回向量****** 
se3 hat= 
        0   -1.5708         0  0.785398
   1.5708         0        -0 -0.785398
       -0         0         0         0
        0         0         0         0
so3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
SE3 update = 
2.22045e-16          -1           0      1.0001
          1 2.22045e-16           0           0
          0           0           1           0
          0           0           0           1

二、真实轨迹和估计轨迹的绝对误差（包含绘制估计轨迹和真实轨迹）

该程序实现的是计算真实轨迹和估计轨迹的绝对误差，同时利用pangolin/OpenGL画出真实和估计轨迹
注：要用到Eigen、pangolin、Sophus，请提前装好
源码如下：

//
//
//该程序演示了绝对轨迹误差
#include<iostream>
#include<unistd.h>
#include <fstream>
#include "pangolin/pangolin.h"
#include "sophus/se3.hpp"
using namespace Sophus;
using namespace std;
//这个地方我放的是绝对路径
string  groundtruth_file="/home/nnz/data/slam_pratice/pratice_sophus/example/groundtruth.txt";//真是的位姿轨迹文件
string  edtimated_file="/home/nnz/data/slam_pratice/pratice_sophus/example/estimated.txt";//估计的位姿轨迹文件
//利用typedef定义一个容器类型，里面装的是位姿(time 平移，四元数【旋转】)
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(edtimated_file);
    //assert宏的原型定义在<assert.h>中，其作用是如果它的条件返回错误，则终止程序执行，原型定义:
    //#include <assert.h>
    //void assert( int expression );
    //　assert的作用是现计算表达式 expression ，如果其值为假（即为0）
    // 那么它先向stderr打印一条出错信息，然后通过调用 abort 来终止程序运行
    assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());
    //计算 绝对轨迹误差 rmse
    //size_t它是一种“整型”类型，里面保存的是一个整数，就像int, long那样。
    // 这种整数用来记录一个大小(size)。size_t的全称应该是size type，就是说“一种用来记录大小的数据类型”
    //通常我们用sizeof(XXX)操作，这个操作所得到的结果就是size_t类型。
    // 因为size_t类型的数据其实是保存了一个整数，所以它也可以做加减乘除，也可以转化为int并赋值给int类型的变量
    //示例代码：
    //int i;                   // 定义一个int类型的变量i
    //size_t size = sizeof(i); // 用sizeof操作得到变量i的大小，这是一个size_t类型的值
    //                         // 可以用来对一个size_t类型的变量做初始化
    //i = (int)size;           // size_t类型的值可以转化为int类型的值
    double rmse=0;
    for(std::size_t i=0;i<estimated.size();i++)
    {
        Sophus::SE3d p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
        double error =(p2.inverse()*p1).log().norm();//模长，见slam十四讲89页 公式4.44
        rmse+=error*error;//得到模长的平方
    }
    rmse=rmse/double(estimated.size());
    rmse=sqrt(rmse);
    cout<<"绝对误差 rmse= "<< rmse<<endl;
    DrawTrajectory(groundtruth,estimated);
    return  0;
}
TrajectoryType ReadTrajectory(const string &path)
{
    ifstream fin(path);//流入
    TrajectoryType trajectory;
    if(!fin)
    {
        cerr<<"could not find a trajectory file"<<endl;//cerr
        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;
}
void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti)
{
// 创建 pangolin 窗口，画轨迹
    //creat and set titles 、 width 、height of windows
    pangolin::CreateWindowAndBind(" Trajectory form truth and estimated ", 1024, 768);
    //glEnable(GL_DEPTH_TEST)启用了之后，OpenGL在绘制的时候就会检查，当前像素前面是否有别的像素，如果别的像素挡道了它，那它就不会绘制，也就是说，OpenGL就只绘制最前面的一层。
    //当我们需要绘制透明图片时，就需要关闭它glDisable(GL_DEPTH_TEST);
    //并且打开混合 glEnable(GL_BLEND);
    glEnable(GL_DEPTH_TEST);//??
    glEnable(GL_BLEND);//??
    //glBlendFunc(GLenum sfactor,GLenum dfactor);
    //源因子和目标因子是可以通过glBlendFunc函数来进行设置的。
    // glBlendFunc有两个参数，前者sfactor表示源因子，后者dfactor表示目标因子。
    //前者sfactor表示源颜色，后者dfactor表示目标颜色
    //GL_SRC_ALPHA：表示使用源颜色的alpha值来作为因子
    //GL_ONE_MINUS_SRC_ALPHA：表示用1.0减去源颜色的alpha值来作为因子（1-alpha)
    glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

    //OpenGlRenderState 创建一个相机的观察视图，模拟相机
    pangolin::OpenGlRenderState s_cam(//定义投影和初始模型视图矩阵
            //ProjectionMatrix 前两个参数是相机的宽高，紧接着四个参数是相机的内参，最后两个是最近和最远视距
            pangolin::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)
    );
    //在窗口创建交互式视图
    pangolin::View &d_cam=pangolin::CreateDisplay().SetBounds(0.0,1.0,pangolin::Attach::Pix(1.0),1.0,-1024.0f/768.0f).
            //SetBounds() 前四个参数是视图在视窗中的范围（下 上 左 右），建议动手改改参数就明白了
            SetHandler(new pangolin::Handler3D(s_cam));
    //     SetHandle设置相机的视图句柄，需要用它来显示前面设置的 “相机” 所 “拍摄” 的内容
    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< gt.size();i++)
        {
            glColor3f(0.0f, 0.0f, 1.0f);//blue  for 真实轨迹
            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();
        }

        for(size_t i=0;i<esti.size();i++)
        {
            glColor3f(1.0f, 0.0f, 0.0f);  // red for 估计轨迹
            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);//休眠5ms
    }
}

运行结果如下：

绝对误差 rmse= 2.20728