视觉SLAM理论与实践3

一、群的性质

1）{Z，+}是整数集上的加法运算，根据以上群的四条性质，随便带入两个整数显然满足封闭性、结合律、幺元、逆的条件。所以{Z，+}是群。

2）{N，+}是非负整数(或自然数）组成集合上的加法运算，这里如果a=0,那么第四个“逆”的条件a的逆为 “1/0 ”，显然没有意义，不满足“逆”的条件，所以{N，+}不是群。

二、验证向量叉乘的李代数性质

证明过程如下：

三、推导SE(3)的指数映射

证明过程如下：

四、伴随

表1：

表2：

证明过程如下：

五、轨迹的描绘

我们通常会记录机器人的运动轨迹,来观察它的运动是否符合预期。大部分数据集都会提供标准轨迹以供参考,如 kitti、TUM-RGBD 等。这些文件会有各自的格式,但首先你要理解它的内容。记世界坐标系为 W ,机器人坐标系为 C,那么机器人的运动可以用 T W C 或 T CW 来描述。现在,我们希望画出机器人在世界当中的运动轨迹,请回答以下问题:
1.事实上,T W C 的平移部分即构成了机器人的轨迹。它的物理意义是什么?为何画出 T W C 的平移部分就得到了机器人的轨迹?
答：坐标的变换就是Twc的平移部分，即机器人移动的距离，因为解算位姿的时候，是解算两帧之间的位姿，包括平移和旋转，平移即移动距离，所以画轨迹就是将Twc描绘出来。
2.根据轨迹文件(code/trajectory.txt)绘制出运动轨迹，代码详见code/draw_trajectory.cpp和CMakeLists.txt ,运行环境为ubuntu16.04。
[code/draw_trajectory.cpp]:

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "../trajectory.txt";
// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
int main(int argc, char **argv) {
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;//读出的位姿存入该容器类vector中
    /// implement pose reading code
    // start your code here (5~10 lines)
    //*c++中getline()函数/
    ifstream infile(trajectory_file);
    double t1,tx,ty,tz,qx,qy,qz,qw;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    //从string读取数据
            record>>t1>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
            Eigen::Vector3d t(tx,ty,tz);
            Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();  //四元数的顺序要注意
            Sophus::SE3 SE3_qt(q,t);
            poses.push_back(SE3_qt);
        }
    }
    else
    {
        cout<<"没找到这个文件"<<endl;
    }
    infile.close();
     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);   // sleep 5 ms
    }
}

CMakeLists.txt :

cmake_minimum_required(VERSION 2.8)
project(draw_trajectory)
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
set( CMAKE_BUILD_TYPE "Debug" )

find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
        ${Pangolin_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIR}
)
add_executable(trajectory draw_trajectory.cpp)
target_link_libraries(trajectory
        ${Pangolin_LIBRARIES}
        ${Sophus_LIBRARIES}
        )

运行结果如下：

六、轨迹误差

除了画出真实轨迹以外,我们经常需要把 SLAM 估计的轨迹与真实轨迹相比较。误差定义为：

即两个位姿之差的李代数二范数。于是，可以定义两条轨迹的均方根（Root-Mean-Square-Error, RMSE）误差为：

代码详见code1/compare_tra.cpp和CMakeLists.txt ，环境ubun16.04,详细描述如下：

compare_tra.cpp:

//
// Created by chengjun on 2019/11/20.
//
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string gt_file = "../groundtruth.txt";
string est_file = "../estimated.txt";
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_poses,est_poses;
// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_poses,
                    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_poses);
void readData(string filepath);
void ErrorTrajectory();
int main(int argc, char **argv) {
    readData(gt_file);
    readData(est_file);
    ErrorTrajectory();
    // draw trajectory in pangolin
    DrawTrajectory(gt_poses,est_poses);//打印两条轨迹
    return 0;
    /// implement pose reading code
    // start your code here (5~10 lines)
}
/*******************************************************************************************/
void readData(string filepath){
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
    ifstream infile(filepath);
    double t1,tx,ty,tz,qx,qy,qz,qw;
    string line;
    if(infile)
    {
        while(getline(infile,line))
        {
            stringstream record(line);    //从string读取数据
            record>>t1>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
            Eigen::Vector3d t(tx,ty,tz);
            Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();  //四元数的顺序要注意
            Sophus::SE3 SE3_qt(q,t);
            poses.push_back(SE3_qt);
        }
    }
    else
    {
        cout<<"没找到这个文件"<<endl;
    }
    if(filepath==gt_file){
        gt_poses = poses;
    }else if( filepath==est_file ){
        est_poses = poses;
    }else{ cout<<"读文件出错"<<endl;}
    infile.close();
}
/*******************************************************************************************/
void ErrorTrajectory()
{
    double RMSE = 0;
    Eigen::Matrix<double ,6,1> se3;
    vector<double> error;
    for(int i=0;i<gt_poses.size();i++){
        se3=(gt_poses[i].inverse()*est_poses[i]).log();  //这里的se3为向量形式，求log之后是向量形式
        //cout<<se3.transpose()<<endl;
        error.push_back( se3.squaredNorm() );  //二范数
        // cout<<error[i]<<endl;
    }
    for(int i=0; i<gt_poses.size();i++){
        RMSE += error[i];
    }
    RMSE /= double(error.size());
    RMSE = sqrt(RMSE);
    cout<<RMSE<<endl;
}
/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_poses,
                    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_poses) {
    if (gt_poses.empty()) {
        cerr << "groundtruth is empty!" << endl;
        return;
    }
    if (est_poses.empty()) {
        cerr << "estimated 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);//窗口,rgba
        glLineWidth(2);
        for (size_t i = 0; i < est_poses.size() - 1; i++) {
            glColor3f(1 - (float) i / est_poses.size(), 0.0f, (float) i / est_poses.size());
            glBegin(GL_LINES);
            auto p1 = est_poses[i], p2 = est_poses[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 < gt_poses.size() - 2; i++) {
            glColor3f(0.f, 0.8f, 0.f);//绿色
            glBegin(GL_LINES);
            auto p3 = gt_poses[i], p4 = gt_poses[i + 1];//只显示tx,ty,tz
            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);   // sleep 5 ms
    }
}

CmakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(draw_trajectory)

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

find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
        ${Pangolin_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIR}
)
add_executable(tra campare_tra.cpp)
target_link_libraries(tra
        ${Pangolin_LIBRARIES}
        ${Sophus_LIBRARIES}
        )

运行结果如下：

最后计算出来的RMSE值为2.20728.

友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！