视觉slam14讲学习（一）之se3上的定位表示:轨迹显示与轨迹误差_高翔轨迹文件trajectory.txt

作者：小小林熬夜学编程 | 2024-02-18 00:50:11

踩

高翔轨迹文件trajectory.txt

文章目录

1.读出trajectory.txt中的轨迹信息

高翔博士的视觉slam14讲书籍下载资源
如何描述视觉定位的精度？一般会用定位误差来描述，有很多开源工具干这件事情，在这之前，我们先学习如何用Pangolin来画出机器人的定位轨迹。
首先需要读出trajectory.txt中的轨迹信息，其中txt中的轨迹格式是[time,tx,ty,tz,qx,qy,qz,qw]

#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中


//读取轨迹文件中的位姿，T（t3,q4）
    //第一种方法，用fstream的getline分行读取stringstream按空格拆分传入数组
   /* ifstream infile;
    infile.open(trajectory_file, ios::in);
    if(!infile.is_open())
    cout<<"open file failture"<<endl;
    string line;
    while(!infile.eof() && std::getline(infile, line)){//是否读到文件的最后,是否读取一行
        stringstream ss(line);//使用getlne分行，使用stringstream切割空格
        double str;
        vector<double> arr;
        while(ss >> str){//传入字符
            cout<<str<<endl;
            arr.push_back(str);//传入数组arr
        }
        Eigen::Quaterniond q(arr[7], arr[8], arr[5], arr[6]);
        Eigen::Vector3d t(arr[1], arr[2], arr[3]);
        Sophus::SE3 SE3(q,t);
        poses.push_back(SE3);
        cout<<endl;
        cout<<line<<endl;
    }
    infile.close();*/
   //*第二种方法，参考点云地图传入,gaoxiang书的第五讲/
    ifstream in(trajectory_file);//创建输入流
   
    if(!in){
        cout<<"open posefile failture!!!"<<endl;
        return 0;
    }
    for(int i=0; i<620; i++){

        double data[8]={0};
        for(auto& d:data) in>>d;//按行依次去除数组中的值

        Eigen::Quaterniond q(data[7], data[8], data[5], data[6]);
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3(q,t);
        poses.push_back(SE3);

               
    }
   //*/

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

            
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

2. 用pangolin画出轨迹poses

//gaoxiang提供的画轨迹的函数
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);//窗口，rgba

        glLineWidth(2);//线宽
        //cout<<"pose.size()="<<poses.size();
        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];//只显示tx,ty,tz
            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
    }

}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

3. 利用Eigen进行欧拉角和四元数的转化

Eigen::AngleAxisd rollAngle(AngleAxisd(roll_ - roll1_,Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(pitch_ - pitch1_,Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(yaw_ - yaw1_,Vector3d::UnitZ()));
Eigen::Quaterniond q;
q= yawAngle * pitchAngle * rollAngle;
q.normalized();

4. 画出两条轨迹，对定位精度进行分析

对于第i位姿误差定义： $ei=\parallel$ log(Tqt $^{-1}$ .Test) $^\upsilon$ $\parallel$ （从4x4的矩阵变成6x1的向量）;
总的误差和： $RMSE=\sqrt{1/n\sum_{i=0}^{n}ei^{2}}$

//author:jiangcheng
#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 = "/home/ubuntu/DL/深蓝slam/L4/draw_trajectory/groundtruth.txt";
string est_file = "/home/ubuntu/DL/深蓝slam/L4/draw_trajectory/estimated.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> get_pose(string& pose_file);

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

void compare_difference(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,
                        vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses);

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

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_pose=get_pose(gt_file);
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_pose=get_pose(est_file);//继续往poses全局变量里面传数据

  
   // draw trajectory in pangolin
    //DrawTrajectory(gt_pose,est_pose);//打印两条轨迹


    //计算误差
    compare_difference(gt_pose,est_pose);
    return 0;
}


/****************************************************************************************/
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> get_pose(string& pose_file){
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;//读出的位姿存入该容器类vector中,局部变量
    //*第二种方法，参考点云地图传入,gaoxiang书的第五讲/
   
    ifstream in(pose_file);//创建输入流
   
    if(!in){
        cout<<"open posefile failture!!!"<<endl;
        return poses;
    }
    for(int i=0; i<620; i++){

        double data[8]={0};

        for(auto& d:data) in>>d;//按行依次去除数组中的值
        Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
        Eigen::Vector3d t(data[1], data[2], data[3]);
        Sophus::SE3 SE3(q,t);
        poses.push_back(SE3);
          
    }
    return poses;
}


/*******************************************************************************************/
//计算两个轨迹的误差
void compare_difference(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,
                    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses){
 
    double rmse_square=0;
    double rmse=0;

    for (int i = 0; i <612; i++) {


            auto p1 = gt_poses[i];
            Sophus::SE3 p2 = est_poses[i];
            cout<<"p1.matrix "<<p1.matrix()<<"\n,p2.matrix"<<p2.matrix()<<endl;
            Eigen::Matrix<double,4,4> m = (p1.matrix().inverse())*p2.matrix();
            
            cout<<"m.matrix"<<m.matrix()<<endl;
            Eigen::Matrix<double,3,3> R = m.topLeftCorner<3,3>();
            Eigen::Matrix<double,3,1> t = m.topRightCorner<3,1>();
            Sophus::SE3 SE3_dot(R,t);//构造T12的李群SE3，从4x4的矩阵变成6x1的向量
            cout<<"se3 is "<<SE3_dot.matrix()<<endl;//打印矩阵形式
            //李代数是6维向量，se3
            Sophus::Vector6d se3_log= SE3_dot.log();//对数映射
            
            //累加误差
            double error=se3_log.squaredNorm();//二范数的平方,squaredNorm;二范数，norm()
            cout<<"se3 squarNorm is "<<error<<endl;
            rmse_square=rmse_square+error;//累加
        }
    rmse=sqrt((rmse_square)/612);
    cout<<"RSME is :"<<rmse<<endl;
}



/*******************************************************************************************/
//gaoxiang提供的画轨迹的函数,画两条轨迹
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() || est_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);//窗口，rgba

        glLineWidth(2);//线宽
        //cout<<"pose.size()="<<poses.size();
        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];//只显示tx,ty,tz
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);

            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
    }

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157

5.结果显示

两条轨迹显示
误差计算显示

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/102985