视觉slam14讲学习（一）之se3上的定位表示:轨迹显示与轨迹误差

最新推荐文章于 2024-03-22 22:15:24 发布

su扬帆启航

最新推荐文章于 2024-03-22 22:15:24 发布

阅读量3.3k

点赞数 2

分类专栏：视觉slam学习文章标签：视觉slam

本文链接：https://blog.csdn.net/orange_littlegirl/article/details/88418954

版权

视觉slam学习专栏收录该内容

28 篇文章 47 订阅

订阅专栏

文章目录

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位姿误差定义： $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
    }

}