SLAM学习笔记

编译环境ubuntu20.04,vs code

（李群 李代数）

先是CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(learning_sophus)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_BUILD_TYPE "Release")

find_package(Sophus REQUIRED)

include_directories("/usr/inlcude/eigen3")
include_directories("Panglin_INCLUDE_DIRS")
add_executable(use useSophus.cpp)
add_executable(RMSE trajectoryError.cpp)

target_link_libraries(use Sophus::Sophus)

find_package( Pangolin )

target_link_libraries(RMSE ${Pangolin_LIBRARIES})
target_link_libraries(RMSE Sophus::Sophus)

主要是一些简单的李群李代数计算（使用Sophus库），还有对轨迹误差作绝对轨迹误差求解

Sophus简单使用

#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)
{
    Matrix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();              //旋转90度，先用轴角的形式声明一个旋转向量，然后转换成旋转矩阵赋值给R

    Quaterniond q(R);                                                                //用四元数表达旋转
    Sophus::SO3d SO3_R(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;

    Vector3d so3 = SO3_R.log();                                                      //对数映射
    cout<<"so3 = "<<so3.transpose()<<endl;                                           //旋转向量和旋转矩阵，指数映射就是罗德里格斯公式

    cout<<"so3_hat =\n"<<Sophus::SO3d::hat(so3)<<endl;                               //hat将向量转换为反对称矩阵

    cout<<"so3_hat vee= "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;//vee为逆运算，即从反对称矩阵到向量

//扰动更新-----------------------------------------------------------------------------------------------------------------------------------
    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;

    Vector3d t(1,0,0);                                                               //平移一个单位（x轴）
    Sophus::SE3d SE3_Rt(R,t);                                                        //两种不同的方式构造李群
    Sophus::SE3d SE3_qt(q,t);
    cout<<"SE3 from R,t=\n"<<SE3_Rt.matrix()<<endl;
    cout<<"SE3 from q,t =\n"<<SE3_qt.matrix()<<endl;

    typedef Eigen::Matrix<double ,6 ,1> Vector6d;                                    //声明一个6*1的向量模板

    Vector6d se3 = SE3_Rt.log();                                                     //对数映射构造李代数
    cout<<"se3 = "<<se3.transpose()<<endl;
    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-4;                                                          //设置六维向量表示的李代数的第一个值
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;                 //扰动对李群的更新（李代数用指数映射）
    cout<<"SE3_updated = "<<endl
        <<SE3_updated.matrix()<<endl;

    return 0; 

}

轨迹误差

#include <iostream>
#include <fstream>
#include <cmath>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
using namespace std;
using namespace Sophus;

string groundtruth_file = "/home/martin/桌面/code/sophus/groundtruth.txt";
string estimated_file = "/home/martin/桌面/code/sophus/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);

void RMSE(TrajectoryType &estimated,TrajectoryType &groundtruth);

TrajectoryType Read_Translatin(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.size());                                       //和之前的未读取到文件cout一个声明同一个功能
    
    RMSE(estimated,groundtruth);
   
    DrawTrajectory(groundtruth,estimated);

    TrajectoryType groundtruth1 = Read_Translatin(groundtruth_file);                      //相对平移误差（我自己的理解，可能不对）
    TrajectoryType estimated1 = Read_Translatin(estimated_file);
    RMSE(estimated1,groundtruth1);
    
    return 0;
}

void RMSE(TrajectoryType &estimated,TrajectoryType &groundtruth)                         //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();                                   //norm求范数
        rmse += error*error;      
    }

    rmse = rmse/double(estimated.size());
    
    rmse = sqrt(rmse);

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



TrajectoryType ReadTrajectory(const string &path)                                         //读取轨迹数据
{
    ifstream fin(path);
    TrajectoryType trajectory;
    if(!fin)
    {
        cerr<<"trajectory "<< path << "not found. "<<endl;                                //功能上和cout类似，cerr是不缓冲的，用于输出错误信息，详细情况查api或者csdn上搜
        return trajectory;
    }

    while (!fin.eof())                                                                    //文件读取和vector的尾插操作
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qw,qx,qy,qz),Eigen::Vector3d(tx,ty,tz));       //书中有误四元数顺序是实部w，虚部x，y，z
        trajectory.push_back(p1);
    }

    return trajectory;
    
}

TrajectoryType Read_Translatin(const string &path)
{
    ifstream fin(path);
    TrajectoryType trajectory;
    if(!fin)
    {
        cerr<<"trajectory "<< path << "not found. "<<endl;                                //功能上和cout类似，cerr是不缓冲的，用于输出错误信息，详细情况查api或者csdn上搜
        return trajectory;
    }

    while (!fin.eof())                                                                    //文件读取和vector的尾插操作
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Sophus::SE3d p1(Eigen::Quaterniond(1,0,0,0),Eigen::Vector3d(tx,ty,tz));       //书中有误四元数顺序是实部w，虚部x，y，z
        trajectory.push_back(p1);
    }

    return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti)                   //pangolin显示轨迹
{
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024,768 );                         //标题和窗口大小
    glEnable(GL_DEPTH_TEST);                                                               //这三句直接写 这句是启用深度检测，下一句是启用混合
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC0_ALPHA,GL_ONE_MINUS_SRC_ALPHA);                                     //表示颜色的混合方式

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),                     //投影矩阵（前四个是尺寸，然后是near和far的边界值）
        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);                                                             //激活相机s
        
        glClearColor(0.0f,0.0f,0.0f,0.0f);                                                 //opencv类似设定颜色和线宽(背景颜色设置，我觉得黑色明显一点全是1.0f就是白色了)
        
//和上一次比少了那些姿态的刻画，在这次里姿态不是主要的，主要看两条轨迹之间的误差----------------------------------------------------------------------------------------
        
        for (size_t i = 0; i < gt.size(); i++)                                             //画线
        {
            glColor3f(1.0,0.0,0.0);                                                        //实际为红色
            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]);

            glColor3f(0.0,1.0,0.0);                                                        //估计为绿色
            auto q1 = esti[i],q2 = esti[i+1];
            glVertex3d(q1.translation()[0],q1.translation()[1],q1.translation()[2]);
            glVertex3d(q2.translation()[0],q2.translation()[1],q2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000);
    }
}

还得继续看李代数这块的知识