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.txt和KeyFrameTrajectory.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;
}