视觉slam14讲学习（一）之se3上的定位表示:轨迹显示与轨迹误差
文章目录
- 1.读出trajectory.txt中的轨迹信息
- 2. 用pangolin画出轨迹poses
- 3. 利用Eigen进行欧拉角和四元数的转化
- 4. 画出两条轨迹，对定位精度进行分析
- 5.结果显示
1.读出trajectory.txt中的轨迹信息
高翔博士的视觉slam14讲书籍下载资源
如何描述视觉定位的精度？一般会用定位误差来描述，有很多开源工具干这件事情，在这之前，我们先学习如何用Pangolin来画出机器人的定位轨迹。
首先需要读出trajectory.txt中的轨迹信息，其中txt中的轨迹格式是[time,tx,ty,tz,qx,qy,qz,qw]
#include
#include
#include
#include
// need pangolin for plotting trajectory
#include
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>);
int main(int argc, char **argv) {
vector> poses;//读出的位姿存入该容器类vector中
//读取轨迹文件中的位姿，T（t3,q4）
//第一种方法，用fstream的getline分行读取stringstream按空格拆分传入数组
/* ifstream infile;
infile.open(trajectory_file, ios::in);
if(!infile.is_open())
cout<<"open file failture"< arr;
while(ss >> str){//传入字符
cout<>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> 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()="< 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
#include
#include
#include
// need pangolin for plotting trajectory
#include
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> get_pose(string& pose_file);
void DrawTrajectory(vector> >_poses,
vector> &est_poses);
void compare_difference(vector> >_poses,
vector> &est_poses);
int main(int argc, char **argv) {
vector> gt_pose=get_pose(gt_file);
vector> 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> get_pose(string& pose_file){
vector> poses;//读出的位姿存入该容器类vector中,局部变量
//*第二种方法，参考点云地图传入,gaoxiang书的第五讲/
ifstream in(pose_file);//创建输入流
if(!in){
cout<<"open posefile failture!!!"<>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> >_poses,
vector> &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 "< m = (p1.matrix().inverse())*p2.matrix();
cout<<"m.matrix"< R = m.topLeftCorner<3,3>();
Eigen::Matrix t = m.topRightCorner<3,1>();
Sophus::SE3 SE3_dot(R,t);//构造T12的李群SE3，从4x4的矩阵变成6x1的向量
cout<<"se3 is "<> >_poses,
vector> &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()="< 5.结果显示
- 两条轨迹显示
- 误差计算显示
