slam十四讲评价轨迹的误差（trajectoryError）

trajectoryError.cpp

#include <sophus/se3.hpp> 
#include <unistd.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
using namespace std;
using namespace Eigen;
 
void ReadData(string FileName ,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses);
double ErrorTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g,
        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e);
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g,
        vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e);
 
int main(int argc, char **argv)
{
    string GroundFile = "../groundtruth.txt";
    string ErrorFile = "../estimated.txt";
    double trajectory_error_RMSE = 0;
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g;
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e;
 
    ReadData(GroundFile,poses_g);
    ReadData(ErrorFile,poses_e);
    trajectory_error_RMSE = Error