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;
}
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
}
}
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位姿误差定义:
e
i
=
∥
ei=\parallel
ei=∥log(Tqt
−
1
^{-1}
−1.Test)
υ
^\upsilon
υ
∥
\parallel
∥(从4x4的矩阵变成6x1的向量);
总的误差和:
R
M
S
E
=
1
/
n
∑
i
=
0
n
e
i
2
RMSE=\sqrt{1/n\sum_{i=0}^{n}ei^{2}}
RMSE=1/n∑i=0nei2
//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>> >_poses,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses);
void compare_difference(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> >_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>> >_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>> >_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
}
}
5.结果显示
- 两条轨迹显示
- 误差计算显示