转载：https://www.cnblogs.com/feifanrensheng/p/8628900.html
使用 code/ground-truth.txt 和 code/estimate.txt 两条轨迹。请你根据上面公式,实现 RMSE
的计算代码,给出最后的 RMSE 结果。作为验算,参考答案为:2.207。用上题的画图程序将两条轨迹画在同一个图里,看看它们相差多少。
完整题目描述
ground-truth.txt和estimate.txt放在了源文件夹下的data目录下，编写了用于画图的trajectory_compare.cpp文件
相关代码及程序可在我的github中获取，地址：https://github.com/feifanrensheng/trajectory_compare
代码如下：
1 // trajectory_compare.cpp created by zhang ning 2018/3/23
2 #include <sophus/se3.h>
3 #include <string>
4 #include <iostream>
5 #include <fstream>
6
7 // need pangolin for plotting trajectory
8 #include <pangolin/pangolin.h>
9
10 using namespace std;
11
12
13 // function for plotting trajectory, don't edit this code
14 // start point is red and end point is blue
15 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
16
17 int main(int argc, char **argv) {
18
19 vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,poses2;
20
21 /// implement pose reading code
22 // start your code here (5~10 lines)
23 ifstream infile;
24 infile.open("../data/estimated.txt");
25 if(!infile) cout<<"error"<<endl;
26
27 cout<<"存入数组"<<endl; //先将文件中的数据存入到一个二维数组中
28 double data;
29 double a[612][8];
30 double *p=&a[0][0];
31 while(infile>>data) //遇到空白符结束
32 {
33 *p=data;
34 p++;
35 }
36 infile.close();
37 for(int i=0;i<620;i++) //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中
38 {
39 Eigen::Quaterniond q1 = Eigen::Quaterniond(a[i][7],a[i][4],a[i][5],a[i][6]);
40 Eigen::Vector3d t1;
41 t1<<a[i][1],a[i][2],a[i][3];
42 Sophus::SE3 SE3_qt1(q1,t1);
43 poses1.push_back(SE3_qt1);
44 }
45 ifstream truth;
46 truth.open("../data/groundtruth.txt");
47 if(!truth) cout<<"error"<<endl;
48
49 cout<<"存入数组"<<endl; //先将文件中的数据存入到一个二维数组中
50 double data1;
51 double b[612][8];
52 double *p1=&b[0][0];
53 while(truth>>data1) //遇到空白符结束
54 {
55 *p1=data1;
56 p1++;
57 }
58 truth.close();
59 for(int i=0;i<620;i++) //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中
60 {
61 Eigen::Quaterniond q2 = Eigen::Quaterniond(b[i][7],b[i][4],b[i][5],b[i][6]);
62 Eigen::Vector3d t2;
63 t2<<b[i][1],b[i][2],b[i][3];
64 Sophus::SE3 SE3_qt2(q2,t2);
65 poses2.push_back(SE3_qt2);
66 }
67 // end your code here
68
69 // draw trajectory in pangolin
70 DrawTrajectory(poses1,poses2);
71 return 0;
72 }
73
74 /*******************************************************************************************/
75 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses2) {
76 if (poses1.empty() || poses2.empty() ) {
77 cerr << "Trajectory is empty!" << endl;
78 return;
79 }
80
81 // create pangolin window and plot the trajectory
82 //创建一个窗口
83 pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
84 //启动深度测试
85 glEnable(GL_DEPTH_TEST);
86 //启动混合
87 glEnable(GL_BLEND);
88 //混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子
89 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
90 // Define Projection and initial ModelView matrix
91 pangolin::OpenGlRenderState s_cam(
92 pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
93 //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)
94 pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
95 );
96
97 pangolin::View &d_cam = pangolin::CreateDisplay()
98 .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
99 .SetHandler(new pangolin::Handler3D(s_cam));
100
101
102 while (pangolin::ShouldQuit() == false) {
103 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
104
105 d_cam.Activate(s_cam);
106 glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
107
108
109 glLineWidth(2);
110 for (size_t i = 0; i < poses1.size() - 1; i++) {
111 glColor3f(1 - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());
112 glBegin(GL_LINES);
113 auto p1 = poses1[i], p2 = poses1[i + 1];
114 glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
115 glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
116 glEnd();
117 }
118
119 for (size_t j = 0; j < poses2.size() - 1; j++) {
120 glColor3f(1 - (float) j / poses2.size(), 0.0f, (float) j / poses2.size());
121 glBegin(GL_LINES);
122 auto p3 = poses2[j], p4 = poses2[j + 1];
123 glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
124 glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
125 glEnd();
126
127 }
128 pangolin::FinishFrame();
129 usleep(5000); // sleep 5 ms
130 }
131
132 }
#CMakeLists.txt
# writed by zhang ning 2018/3/22
cmake_minimum_required( VERSION 2.8 )
project(trajectory_compare)
set( CMAKE_BUILD_TYPE "Debug" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
find_package( Sophus REQUIRED)
find_package( Pangolin REQUIRED)
include_directories( "/usr/include/eigen3" )
include_directories( ${Sophus_INCLUDE_DIRS} )
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable( trajectory_compare trajectory_compare.cpp)
target_link_libraries( trajectory_compare ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} )
画出效果图如下所示
