【死磕slam14】slambook2第四讲详解trajectoryError.cpp

山风不在岚

于 2022-11-01 15:01:30 发布

阅读量321

点赞数

CC 4.0 BY-SA版权

分类专栏：带读《视觉SLAM14讲》文章标签： c++ 算法开发语言 opencv 计算机视觉

本文链接：https://blog.youkuaiyun.com/zhangyuanbonj/article/details/127633958

本文详细探讨了《slambook2》中关于trajectoryError.cpp的内容，涉及slam（同时定位与建图）的核心算法，重点讲解了如何计算轨迹误差，并介绍了与C++, OpenCV和计算机视觉算法的关联。" 107477934,8765397,"Java CSV解析：OpenCSV, Apache Commons CSV 和 Super CSV

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

trajectoryError.cpp

#include<iostream>
#include<fstream>
#include<unistd.h>
#include<pangolin/pangolin.h>
#include<sophus/se3.hpp>

using namespace std;
using namespace Sophus;
using namespace Eigen;

string groundtruth_file="/home/zhangyuanbo/Slam14_2/ch4-1/src/groundtruth.txt";// groundtruth_file 存储位置
string estimated_file="/home/zhangyuanbo/Slam14_2/ch4-1/src/estimated.txt";

typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>>  TrajectoryType;//容器中元素是SE3d,内存管理方法是Eigen::aligned_allocator<Sophus::SE3d>
void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti);//提前声明函数1

TrajectoryType  ReadTrajectory(const string &path);//提前声明函数2

int main(int argc, char **argv)
{
   
   
    TrajectoryType groundtruth= ReadTrajectory(groundtruth_file);//读取轨迹文件
    TrajectoryType estimated= ReadTrajectory(estimated_file);//读取

    //assert()为断言函数，如果它的条件返回错误，那么终止程序执行
    assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());

    //compute rmse
    //计算绝对轨迹误差，即为李代数的均方根误差
    double rmse=0;
    for(size_t i=0;i<estimated.size();i++)//    size_t size() const
    {
   
   
        S