为了理解RMSE首先介绍一些统计学的概念，
然后介绍SLAM领域里面的计算精度ATE和RPE的用法。

中位数

一组数据按大小顺序排列，位于最中间的一个数据 (当有偶数个数据时，为最中间两个数据的平均数) 叫做这组数据的中位数。

用中位数作为一组数据的代表，可靠性不高，但受极端数据影响的可能性小一些，有利于表达这组数据的 “集中趋势”。

众数

几组数据中出现次数最多的那个数据，叫做这批数据的众数。

用众数作为一组数据的代表，可靠性较差，但众数不受极端数据的影响，并且求法简便，当一组数据中个别数据变动较大时，适宜选择众数来表示这组数据的 “集中趋势”。

平均数

算数平均数是一组数据的和除以这组数据的个数所得的商、反映一组数的总体情况比中位数、众数更为可靠、稳定。

方差(variance)

方差是各数据偏离平均值差值的平方和的平均数。
方差用来度量随机变量和其数学期望(即均值)之间的偏离程度。

标准差\均方差(Standard Deviation)

标准差(也称均方差)的平方就是方差。
标准差能反映一个数据集的离散程度 (或理解为数据集的波动大小)。
方差与我们要处理的数据的量纲是不一致的(单位不一致)，虽然能很好的描述数据与均值的偏离程度，但是处理结果是不符合我们的直观思维的。

均方误差 MSE (mean squared error)

总的来说，方差是数据序列与均值的关系，而均方误差是数据序列与真实值之间的关系，所以我们只需注意区分 真实值和均值 之间的关系就行了。均方误差(MSE)是各数据偏离真实值 差值的平方和 的平均数方差是平均值，均方误差是真实值。

均方根误差 RMSE(Root Mean Squard Error)

均方根误差是均方误差的算术平方根亦称标准误差，
均方误差是各数据偏离真实值差值的平方和的平均数，也就是误差平方和的平均数，均方根误差才和标准差形式上接近。

举个例子：我们要测量房间里的温度，很遗憾我们的温度计精度不高，

所以就需要测量5次，得到一组数据[x1,x2,x3,x4,x5],

假设温度的真实值是x，数据与真实值的误差e=x-xi 。那么均方误差和均方根误差就可以求出来。总的来说，均方差(标准差)是数据序列与均值的关系，而均方根误差是数据序列与真实值之间的关系。因此，标准差是用来衡量一组数自身的离散程度，而均方根误差是用来衡量观测值同真值之间的偏差，它们的研究对象和研究目的不同，但是计算过程类似。

协方差(Covariance)

方差/ 标准差描述的是一维数据集合的离散程度， 但世界上现象普遍是多维数据描述的，那么很自然就会想到现象和数据的相关程度，以及各维度间相关程度。
比如，一个产品卖的好不好有很多因素构成，比如产品质量，价格等。那么价格质量之间是否有相关性呢？这个问题就可以用协方差来解决。

向量的范数

可以从函数、几何与矩阵的角度去理解范数。

我们都知道，函数与几何图形往往是有对应关系的，这个很好想象，特别是在三维以下的空间内，函数是几何图像的数学概括，而几何图像是函数的高度形象化，比如一个函数对应几何空间上若干点组成的图形。

但当函数与几何超出三维空间时，就难以获得较好的想象，于是就有了映射的概念，映射表达的就是一个集合通过某种关系转为另外一个集合。通常数学书是先说映射，然后再讨论函数，这是因为函数是映射的一个特例。

为了更好的在数学上表达这种映射关系，(这里特指线性关系)于是就引进了矩阵。这里的矩阵就是表征上述空间映射的线性关系。而通过向量来表示上述映射中所说的这个集合，而我们通常所说的基，就是这个集合的最一般关系。

于是，我们可以这样理解，一个集合(向量)，通过一种映射关系(矩阵)，得到另外一个集合(另外一个向量)。

那么向量的范数表示这个原有集合的大小。
矩阵的范数表示这个变化过程的大小的一个度量。

简单说：

0范数表示向量中非零元素的个数(即为其稀疏度)。

1范数表示为，绝对值之和。而2范数则指模。

经过前面的铺垫下面才是真正的大boos

ATE：absolute trajectory error 绝对轨迹误差

绝对轨迹误差直接计算相机位姿的真实值与SLAM系统的估计值之间的差，可以非常直观地反应算法精度和轨迹全局一致性。

需要注意的是，估计位姿和groundtruth通常不在同一坐标系中，因此程序首先根据位姿的时间戳将真实值和估计值进行对齐， 然后计算每对位姿之间的差值， 并最终以图表的形式输出， 该标准非常适合于评估视觉 SLAM 系统的性能。

对于双目SLAM和RGB-D SLAM， 尺度统一因此我们需要通过最小二乘法计算一个从估计位姿到真实位姿的转换矩阵SE3；
对于单目相机，具有尺度不确定性，我们需要计算一个从估计位姿到真实位姿的相似转换矩阵sim3 。

ATE-all　RMSE

ATE-all实际上是每个位姿李代数的均方根误差RMSE(Root Mean Squard Error)。这种误差可以刻画两条轨迹的旋转和平移误差。

ATE-trans　RMSE

ATE-trans仅考虑平移误差的情况，trans表示取括号内部标量的平移部分，因为从整条轨迹上看，旋转出现误差后，随后的平移上会出现误差，所以这两种指标在实际中都适用。

RPE：relative pose error 相对位姿误差

相对位姿误差主要描述的是相隔固定时间差两帧位姿差的精度(相比真实位姿)，相当于直接测量里程计的误差。

当然也有人不用RMSE，直接使用平均值、甚至中位数来描述相对误差情况。

需要注意的是，RPE包含两部分误差，分别是旋转误差和平移误差，通常使用平移误差进行评价已经足够，但是如果需要，旋转角的误差也可以使用相同的方法进行统计。

RPE-all RMSE

RPE-trans RMSE

源码解读

#include #include #include #include #include using namespace Sophus;using namespace std;// 更高精度的轨迹 作为你真实轨迹string groundtruth_file = "/home/projects/sophus/trajectoryError/groundtruth.txt";// 算法计算出来的轨迹string estimated_file = "/home/projects/sophus/trajectoryError/estimated.txt";// Twc 的平移部分构成了机器人的轨迹// aligned_allocator管理C++中的各种数据类型的内存方法是一样的// 在C++11标准中,一般情况下定义容器的元素都是C++中的类型,// 在Eigen管理内存和C++11中的方法不一样,需要单独强调元素的内存分配和管理typedef vector<:se3d eigen::aligned_allocator>> TrajectoryType;TrajectoryType ReadTrajectory(const string &path);void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);int main(int argc, char **argv) {
    
      TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);  TrajectoryType estimated = ReadTrajectory(estimated_file);  assert(!groundtruth.empty() && !estimated.empty());  assert(groundtruth.size() == estimated.size());  // compute rmse 位姿的均方根误差  // ATE   double rmse = 0;  for (size_t i = 0; i < estimated.size(); i++) {
    
        Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];    // 李群SE3的对数映射求李代数se3    // 对应视觉SLAM十四讲第二版p89 公式4.44    // .norm();代表二范数的计算过程    double error = (p2.inverse() * p1).log().norm();    rmse += error * error;  }  rmse = rmse / double(estimated.size());  rmse = sqrt(rmse);  cout << "RMSE = " << rmse << endl;  DrawTrajectory(groundtruth, estimated);  return 0;}TrajectoryType ReadTrajectory(const string &path) {
    
      ifstream fin(path);  TrajectoryType trajectory;  if (!fin) {
    
        cerr << "trajectory " << path << " not found." << endl;    return trajectory;  }  while (!fin.eof()) {
    
        double time, tx, ty, tz, qx, qy, qz, qw;    // tx  ty  tz 为Twc的平移部分    // qx  qy  qz  qw 是四元数表示的 Twc的旋转部分 qw 是四元数的实部    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;      // 四元数和平移向量构造李群SE3变换矩阵    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));    trajectory.push_back(p1);  }  return trajectory;}void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &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  }}

交流答疑微信群