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

最新推荐文章于 2023-02-15 17:37:45 发布

seven one

最新推荐文章于 2023-02-15 17:37:45 发布

阅读量1.4k

点赞数 2

文章标签： slam c++

本文链接：https://blog.csdn.net/qq_53833529/article/details/122396076

版权

本文介绍SLAM中用于评估轨迹误差的方法，通过`trajectoryError.cpp`程序计算得到的RMSE值为2.27589，详细讨论了这一指标在SLAM算法中的意义和应用。

摘要由CSDN通过智能技术生成

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