• 【计算轨迹误差】

    评估轨迹的误差

    计算均方根误差
    A T E = 1 N ∑ 1 N ∣ ∣ l o g ( T g r o u n d , t r u t h − 1 T e s t i m a t e , i ) ∣ ∣ 2 2 ATE = \sqrt{\frac{1}{N} \sum_{1}^{N} ||log(T^{-1}_{ground,truth}T_{estimate,i})||^2_2} ATE=N1​1∑N​∣∣log(Tground,truth−1​Testimate,i​)∣∣22​ ​

    给定数据的格式：

    如下三组真实数据数据，分别对应的是时间戳，平移，和四元数表示的旋转

    1. 1305031526.67210007 -0.0355094123046875154 -0.0070967674641926326 0.0100241180501300375 0.747064124448400424 0.448673366467715162 -0.443835884862102847 0.208799213344654372
    2. 1305031526.71219993 -0.0337215082682291722 -0.00720814225260424379 0.00152885084635434598 0.735554936692470185 0.468316402085190431 -0.441780989823346237 0.210874938473073842
    3. 1305031526.77220011 -0.0335672539713542939 -0.00830686082356790756 -0.0142845526529947753 -0.715793317510356131 -0.503173810000826283 0.436339717689256446 -0.209913540067406978

    以下三组是估计出来的数据

    1. 1305031526.67147303 0 0 0 0 0 1 0
    2. 1305031526.70754695 0.00288319499999999986 -0.00466209999999999975 -0.00225430399999999994 0.0106974147769499078 0.00218949395434721002 0.999875286151759579 0.0114098017620960397
    3. 1305031526.77148104 0.0139789660000000007 -0.0130823169999999996 -0.0108695960000000005 0.0325266728104138744 0.00326054208123759126 0.998528028878688856 0.0432800180783373817

    简要分析数据可知，数据的时间是对齐的，有效位数是个位数

    代码编写流程

    ​

    1. 需要的库文件，Sophus，pangolin
    2. 读入数据，判断数据是否真正读入
    3. 进行计算（从四元数构造SE3）
    4. 画图可视化对比
    5. 总结其中的C++语言技巧
    6. bug总结

    代码如下

    #include 
#include 
#include 
//unistd.h为Linux/Unix系统中内置头文件，包含了许多系统服务的函数原型，例如read函数、write函数和getpid函数等。
//其作用相当于windows操作系统的"windows.h"，是操作系统为用户提供的统一API接口，方便调用系统提供的一些服务。
#include 
#include 
#include 
#include 
using namespace std;
using namespace Sophus;

const string file_est = "/home/wpf/Test/study_cpp/chapter41/data/estimated.txt";
const string file_truth = "/home/wpf/Test/study_cpp/chapter41/data/groundtruth.txt";

//由于返回类型太长，所以重新定义一个返回类型
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectortyType;
TrajectortyType ReadTrajectory(const string &path);
//绘图函数只需要传入两个路径的数组就行
void DrawTrajectory(const TrajectortyType &gt,const TrajectortyType &esti);

int main() {
    //第一步，读入数据
    TrajectortyType groundTruth = ReadTrajectory(file_truth);
    TrajectortyType estimated = ReadTrajectory(file_est);
    //使用vector中的算法
    assert(!groundTruth.empty() && !estimated.empty());
    assert(groundTruth.size() == estimated.size());
    //计算
    double rmse = 0;
    for (int i = 0; i < groundTruth.size(); ++i) {
        Sophus::SE3d p_est = estimated[i],p_tru = groundTruth[i];
        double error_1 = (p_tru.inverse()*p_est).log().norm();
        rmse += error_1*error_1;

    }
    rmse = rmse/groundTruth.size();
    rmse = sqrt(rmse);
    cout<<"RMSE:"<<rmse<<endl;
    //出图像
    DrawTrajectory(groundTruth,estimated);
    std::cout << "Hi,wpf!You're the best!" << std::endl;
    return 0;
}
TrajectortyType ReadTrajectory(const string &path){
    ifstream fin(path);
    TrajectortyType trajectorty;
    if(!fin){
        cerr<<"tracjectory"<<path <<" not found"<<endl;
        return trajectorty;
    }
    //如何读数据，数据格式的处理
    //getline是以‘\n’截断，不会忽略空格。
    //eof是以’空格‘或’\n’截断，会忽略一些必不可少的空格，如query"华为 9X"
    /**
     *
     *
     * */
    while(!fin.eof()){
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qw,qx,qy,qz),Eigen::Vector3d(tx,ty,tz));
        trajectorty.emplace_back(p1);
    }
    return trajectorty;
}
void DrawTrajectory(const TrajectortyType &gt, const TrajectortyType &esti) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    //三个格式定义
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 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(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);

        //画出真实轨迹的图像，
        for (size_t i = 0; i < gt.size() - 1; i++) {
            glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
            glBegin(GL_LINES);
            //获取第一个和第二个之间的数据，获取之间的平移向量
            auto p1 = gt[i], p2 = gt[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        //画出预测轨迹，同上
        for (size_t i = 0; i < esti.size() - 1; i++) {
            glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
            glBegin(GL_LINES);
            auto p1 = esti[i], p2 = esti[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
    }
}

    • 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

    对应的CMakeList.txt如下：

    cmake_minimum_required(VERSION 3.20)
set(CMAKE_CXX_STANDARD 14)
project(chapter41)
find_package(Pangolin REQUIRED)
# 由于Sophus库依赖与fmt库，所以安装Sophus之前要先安装fmt
find_package(fmt REQUIRED)
find_package(Sophus REQUIRED)
include_directories("usr/include/eigen3")
include_directories(${Pangolin_INCLUDE_DIRS})

add_executable(chapter41 main.cpp)

target_link_libraries(chapter41 ${Pangolin_LIBRARIES} Sophus::Sophus fmt::fmt)

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13

    1. C++的语言技巧

      （1）文件的读写，

      //getline是以‘\n’截断，不会忽略空格。
//eof是以’空格‘或’\n’截断，会忽略一些必不可少的空格，如query"华为 9X"

      • 1
      • 2

      如果以行为结尾

      使用

      //方式一：getline读取
getline(fin, line); //逐行读取fin打开的文件，保存在line里。(分隔符为 \n)

      • 1
      • 2

      如果以空格和行为结尾，使用

      while(!fin.eof) {  //当fin打开的文件没有读取完时
  fin >> line;   //将以空格和\n分隔的文本存入line中
}

      • 1
      • 2
      • 3
      #include 
std::ifstream fin;        //定义输入文件流对象
fin.open("data/xxx.txt")    //打开当前程序相对路径下的某个文件，还可追加参数std::ios::in等各种
std::string line;  //文件流载体，面向开发人员

//方式一：getline读取
getline(fin, line); //逐行读取fin打开的文件，保存在line里。(分隔符为 \n)

//方式二:!eof()
while(!fin.eof) {  //当fin打开的文件没有读取完时
  fin >> line;   //将以空格和\n分隔的文本存入line中
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8
      • 9
      • 10
      • 11
      • 12

      （2）使用了typedef关键字

      （3）使用了auto 关键字

    2. bug总结

      bug_1

      问题：QUOTE expected

      • 1

      原因是引入的地址没有加入引号

  • 相关阅读:
    spring boot项目修改配置文件后运行报错
    代码随想录算法训练营第23期day7| 454.四数相加II 、383. 赎金信 、15. 三数之和、18. 四数之和
    麒麟移动运行环境(KMRE)——国内首个开源的商用移固融合“Android生态兼容环境”正式开源
    深度学习七 —— BN & LN & IN & GN
    我眼中的IT行业现状与未来趋势
    本博主二哈喇子！特此声明
    一文聊透 Netty 核心引擎 Reactor 的运转架构
    pytorch调整模型学习率
    文字与视频结合效果
    成都东部新区文化旅游体育局莅临国际数字影像产业园参观入驻企业，共促政产交流“零距离”
  • 原文地址：https://blog.csdn.net/Android_WPF/article/details/126003536