SLAM练习题（五）——李群李代数

计算机视觉life从零开始一起学习SLAM 学习笔记

推导李代数小se(3)的指数映射

(题目来自计算机视觉life)
题目:推导李代数小se(3)的指数映射。

我们知道对于大SE(3)，其对应的李代数为小se(3)。其定义如下：

$$\mathfrak{s}\mathfrak{e}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{l}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\mathbf{\varphi }\in \mathfrak{s}\mathfrak{o}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$

证明1：

$$\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}}& 1\end{array}\right]$$

证明2：令$\varphi$=θa，那么

$$\sum _{n=0}^{\infty }\frac{1}{(n+1)!}{\left({\varphi }^{\wedge }\right)}^n=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\triangleq \mathbf{J}$$

知识点:李代数
(下面答案来自星球问答)
答案：

Sophus库练习

**编程练习:**SLAM问题的目标之一就是精确的估计相机运动的轨迹(姿态),如果我们将相机运动的轨迹绘制出来,就可以直观的观察它的运动是否符合预期。给定一个轨迹文件trajectory.txt,该文件的每一行由若干个数据组成,格式为 [time, tx, ty, tz, qx, qy, qz, qw],其中 time 为时间,tx,ty,tz 为平移部分,qx,qy,qz,qw 是四元数表示的旋转部分,请完成数据读取部分的代码,绘制部分代码已经给出。
知识点: 熟悉李代数库Sophus安装及基本操作
熟悉数据流读取基本操作需要安装pangolin,注意异常数据处理
参考答案:代码框架和轨迹数据见: 链接:https://pan.baidu.com/s/1Nm15-bQcFp1g-Pvck_NVkA 密码:n53r

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <pangolin/pangolin.h>
#include <iomanip>
#include <cfloat>

using namespace std;

void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);

string timestamp;
long double tx;
long double ty;
long double tz;
long double qx;
long double qy;
long double qz;
long double qw;

// path to trajectory file
string trajectory_file = "./trajectory.txt";


int main(int argc, char **argv) {

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    /// implement pose reading code
    // 开始你的代码
    
    string str;
    ifstream fin(trajectory_file.c_str());
    if (!fin)
    {
        cout<<"Fail to open"<<trajectory_file<<endl;
        exit(-1);;
    }
    while (!fin.eof())
    {
        getline(fin,str);
        cout<<"str="<<str<<endl;
        if (str.at(0) == '#')
        {
            continue;
        }
        istringstream is(str);	// 字符串流
        is>>timestamp>>setprecision(9)>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        cout<<timestamp<<" "<<tx<<" "<<ty<<" "<<tz<<" "
            <<qx<<" "<<qy<<" "<<qz<<" "<<qw<<" ";
        if(timestamp == "-1.#INF")
        {
            cout<<"WARN: INF ERROR"<<endl;
            continue;
        }
        // SE3:T R/Q
        Eigen::Vector3d t(tx,ty,tz);
        Eigen::Quaterniond q(qw,qx,qy,qz);
        Sophus::SE3 se3(q,t);
        poses.push_back(se3);
    }
    
    

    // 结束你的代码

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

// 无需改动以下绘图程序
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory 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, 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);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[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);   
    }

}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(08_practice)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加eigen
include_directories("/usr/include/eigen3/")

# 添加sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# 添加pangolin
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

# 添加可执行程序
add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory
    ${Sophus_LIBRARIES}
    ${Pangolin_LIBRARIES})

问题：

• 在编译make的时候，出现了

draw_trajectory.cpp:(.text+0x289)：对‘glClearColor’未定义的引用
draw_trajectory.cpp:(.text+0x296)：对‘glLineWidth’未定义的引用……,

检查了所有的头文件也没找到错误的原因，后来发现是CMakeLists.txt写错了链接命令，将${Pangolin_LIBRARIES} 错误拼写成了${Pangolin_LIBSRARIES}。这类错误真的应该细心细心再细心。

• 对于C++中的数据类型：

浮点数采用：基准值+缩放因子来表示

float类型只能表示数字中的前6位或前7位，double类型只能表示数字中的前12或前13位。

• cout的一些用法

cout输出的时候，通常会删除结尾的零，比如将3.23000输出为3.23，cout.self()将覆盖这种行为。Debug的时候要注意这点。

参考：
从零开始一起学习SLAM | 为啥需要李群与李代数？