当前位置:   article > 正文

TUM数据集groundtruth轨迹与估计轨迹统一参考系_tum数据集中的groundtruth

tum数据集中的groundtruth

TUM数据集groundtruth轨迹与估计轨迹(ORBSLAM2运行的结果)统一参考系

参考https://blog.csdn.net/luo870604851/article/details/85006243
阅读ORBSLAM的相关论文并且在UBuntu系统上编译好ORB-SLAM2后,准备运行下程序,获得估计的轨迹后,评估下轨迹的误差。这里运行的是rgbd_dataset_freiburg1_desk数据集,关于深度图和RGB图、估计轨迹和真实轨迹的时间戳的对齐,使用的是associate.py文件,具体可以参考高翔大佬的一起做RGB-D SLAM 第二季 (一)。绘制两者的轨迹后,发现两者完全没有对上,然后看了groudtruth.txtKeyFrameTrajectory.txt文件后,发现估计轨迹是以第一帧坐标系作为参考,而真值轨迹应该是以某个固定的坐标系作为参考。为了能评估轨迹误差,必须要知道两个参考系之间的变换矩阵T。
不在相同参考系下的轨迹

首先考虑的是,在时间戳对齐后,求取第一帧groundtruth轨迹的逆作为T,下图是运行的结果。在论文中给出的是绝对位移误差为0.046m,而这里计算的结果为0.053m,误差比较大。(这个差的还算比较小的了,其它的数据集也试过了,误差更大。)
在这里插入图片描述
在这里插入图片描述

从上面的结果可以看出,直接取第一帧groundtruth轨迹的逆作为T,最后求得的误差要比论文里给出的要大。分析原因,上述两个参考系之间的变换矩阵T只由一帧计算得,存在比较大的误差。因此,可以把所有帧均考虑进来,构建最小二乘问题来估计T,这就是一个ICP问题,在视觉SLAM十四讲的第七章有给出求解方式。这可以使用SVD方法求解,也可以用非线性优化的方式求解。从下面的图中可以看出,估计的绝对平移误差为0.043m,现在比较符合论文中的结果。

在这里插入图片描述
在这里插入图片描述
下面给出读取轨迹文件并统一参考系,显示轨迹,计算轨迹误差的代码。

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h> 
#include <thread>
#include <sophus/se3.hpp>

class Trajectory{
    public:
        void showAssociateTrajectory();
        vector<double> calTrajectoryError();
        void readAssociateTrajectory(const string &strAssociateTrajectoryFile);
        vector<double> mvimageTimeStamps, mvposeTimeStamps, mvtruthTimeStamps;
        vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> mvpose;
        vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> mvgroundTruthPose;
};


vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> pose;
vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> groundTruthPose;

// Input -> The path of associated trcjectory file.
void Trajectory::readAssociateTrajectory( const string &strAssociateTrajectoryFile){

        ifstream fin(strAssociateTrajectoryFile);
        string str;
        if(!fin){
            cerr << "Failed to open trajectory file at:" << strAssociateTrajectoryFile << endl;
            exit(-1);
        }

        
        // Read trajectory file
        double data[7];
        // bool isFirstFrame = true;
        // orbslam保存的轨迹是以第一帧为世界坐标系,需要将groundtruth轨迹的世界坐标变换到估计轨迹的固定坐标
        // Eigen::Isometry3d Tinv;  
        while(!fin.eof()){ 
            double realTimeStamp, truthTimeStamp;
            Eigen::Vector3d v;
            fin >> realTimeStamp;
            if(fin.fail()) {  // 在读数据后判断,避免多读一行数据!!
                break;
            }
            mvposeTimeStamps.push_back(realTimeStamp);


            //读取实际轨迹
            for(size_t i = 0; i < 7; i++){
                fin >> data[i];
            }
            Eigen::Quaterniond q(data[6],data[3],data[4],data[5]);
            Eigen::Isometry3d T(q);
            T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2])); //左乘,相对与旋转前的坐标
            pose.push_back(T);

            fin >> truthTimeStamp;
            //读取轨迹真值
            for(size_t i = 0; i < 7; i++){
                fin >> data[i];
            }
            Eigen::Quaterniond q1(data[6],data[3],data[4],data[5]);
            Eigen::Isometry3d T1(q1);
            T1.pretranslate(Eigen::Vector3d(data[0],data[1],data[2]));
            // if(isFirstFrame){
            //     Tinv = T1.inverse();
            //     cout << "两组轨迹之间的变换矩阵:" << endl;
            //     cout << "T:" << Tinv.matrix() << endl;
            //     isFirstFrame  = false; 
            // }
            mvtruthTimeStamps.push_back(truthTimeStamp);
            // groundTruthPose.push_back(Tinv*T1); 
            groundTruthPose.push_back(T1); 
        }

        // // 将groundtruth轨迹的世界坐标系转换到估计轨迹的世界坐标系下
        Eigen::Vector3d estiMid, truthMid; // 两组点的质心
        vector<Eigen::Vector3d> vEsti, vTruth; // 两组点的去质心坐标
        int N = pose.size();
        for(size_t i = 0; i < N ; i++){
            estiMid += pose[i].translation();
            truthMid += groundTruthPose[i].translation();
        }
        estiMid /= N;
        truthMid /=N;

        vEsti.resize(N);  vTruth.resize(N);
        for(int i = 0; i < N; i++){
            vEsti[i] =  pose[i].translation() - estiMid;
            vTruth[i] = groundTruthPose[i].translation() - truthMid;
        }
        
        Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
        for(size_t i = 0 ; i < N ; i++){
            W += vEsti[i]*(vTruth[i].transpose());
        }

        Eigen::JacobiSVD<Eigen::Matrix3d> svd (W, Eigen::ComputeFullU|Eigen::ComputeFullV);
        Eigen::Matrix3d u = svd.matrixU();
        Eigen::Matrix3d v = svd.matrixV();

        Eigen::Matrix3d R = u * (v.transpose());
        Eigen::Vector3d t = estiMid - R * truthMid;

        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        cout << "两组轨迹之间的变换矩阵:" << endl;
        cout << "R: " << R << endl;   //这两行输出注释掉,结果会出错?!(迷~~)
        cout << "t: " << t << endl;
        // // 方式1 (方式一和方式二T必须初始化成单位矩阵)
        T.translate(t);
        T.rotate(R);
        // // 方式2
        // T.rotate(R);        // 旋转后,translate()是相对旋转后的坐标,pretranslate()是相对于旋转前的坐标
        // T.pretranslate(t);  // 带pre的函数表示左乘,不带的表示右乘
        // // 方式3
        // // T.matrix().block(0, 0, 3, 3) = R;
        // // T.matrix().col(3) = t.homogeneous(); // 将平移向量变成对应的齐次坐标
        // // T.matrix().row(3) = Eigen::Vector4d(0, 0, 0, 1);

        cout << "T:" << T.matrix() << endl;
        for(auto &p: groundTruthPose){
            p = T*p;
        }

        mvpose = pose;
        mvgroundTruthPose = groundTruthPose;
}

void Trajectory::showAssociateTrajectory(){
    assert(pose.size() == groundTruthPose.size()); 
    assert(!pose.empty() && !groundTruthPose.empty());

    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); //创建一个窗口
    glEnable(GL_DEPTH_TEST); //启动深度测试,OpenGL只绘制最前面的一层,绘制时检查当前像素前面是否有别的像素,如果别的像素挡住了它,那它就不会绘制
    glEnable(GL_BLEND); //在openGL中使用颜色混合
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //选择混合选项

    // 1. 定义相机投影模型:ProjectionMatrix(w, h, fu, fv, u0, v0, zNear, zFar)
    // 2. 定义观测方位向量:
    // 观测点位置:(mViewpointX mViewpointY mViewpointZ)
    // 观测目标位置:(0, 0, 0)
    // 观测的方位向量:(0.0,-1.0, 0.0)
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 289, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
        );

    // 定义地图面板
    // 前两个参数(0.0, 1.0)表明宽度和面板纵向宽度和窗口大小相同
    // 中间两个参数(pangolin::Attach::Pix(175), 1.0)表明右边所有部分用于显示图形
    // 最后一个参数(-1024.0f/768.0f)为显示长宽比
    pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0 ,0.0 , 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(red, green, blue, alpha),数值范围(0, 1)
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glLineWidth(2);

        //画出连线
        for(size_t i = 0; i < pose.size(); i++){
            glColor3f(0.0, 0.0, 0.0);
            glBegin(GL_LINES);
            auto p1 = pose[i], p2 = pose[i+1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
            glColor3f(1.0, 0.0, 0.0); //红色是参考轨迹
            glBegin(GL_LINES);
            p1 = groundTruthPose[i];
            p2 = groundTruthPose[i+1];
            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

    }
}

vector<double> Trajectory::calTrajectoryError(){
    assert(pose.size() == groundTruthPose.size()); 
    assert(!pose.empty() && !groundTruthPose.empty());
    double ATE_all = 0, ATE_trans = 0;
    vector<double> ATE;
    for(size_t i = 0; i < pose.size() ; i++){
        Sophus::SE3d p1(pose[i].rotation(),pose[i].translation());
        Sophus::SE3d p2(groundTruthPose[i].rotation(),groundTruthPose[i].translation());
        double error = ((p2.inverse() * p1).log()).norm();
        ATE_all += error * error;
        error = (groundTruthPose[i].translation() - pose[i].translation()).norm();
        ATE_trans += error*error;
    }
    ATE_all = ATE_all / double(pose.size());
    ATE_all = sqrt(ATE_all);
    ATE_trans = ATE_trans / double(pose.size());
    ATE_trans = sqrt(ATE_trans);
    ATE.push_back(ATE_all);
    ATE.push_back(ATE_trans);
    return ATE;
}
  • 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
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/凡人多烦事01/article/detail/619546
推荐阅读
相关标签
  

闽ICP备14008679号