使用Sophus和pangolin实现轨迹误差

注：SLAM十四将的实例

绝对轨迹误差（Absolute Trahextiry,ATE）

姿态的真实值与姿态的理想值之间的差，实际上是李代数的均方根误差（Root-Mean-Aquared,RMSE）
$$AT{E}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N||log(T_{gt,i}^{-1}{T}_{esti,i}^{\bigvee })|{|}_2^2}$$

绝对平移误差（Average Translational Error）

$$AT{E}_{trans}=\sqrt{\frac{1}{N}\sum _{i=1}^N||trans(T_{gt,i}^{-1}{T}_{esti,i}^{\bigvee })|{|}_2^2}$$

相对位姿误差（Average Translational Error）

$$AT{E}_{trans}=\sqrt{\frac{1}{N-\Delta t}\sum _{i=1}^N||log(T_{gt,i}^{-1}{T}_{gt,i+\Delta t}^{\bigvee }{)}^{-1}(T_{esti,i}^{-1}{T}_{esti,i+\Delta t}^{\bigvee }{)}^{}|{|}_2^2}$$
平移部分真实值与姿态的理想值之间的差
实现：

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/geometry.hpp>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <iostream>
#include <fstream>
#include <cmath>

#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;
using namespace Sophus;
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d >> TrajectoryType;

TrajectoryType read_data(string path, vector<double> &Time);
void plot_jectory( TrajectoryType true_pose,const TrajectoryType pose);
int main(int argc,char **argv)
{
  TrajectoryType data,real_data;
  vector<double> Time_data,Time_real_data;
  // Sophus::SE3d p1,p2;
  data=read_data("/home/n1/notes/Sophus/trajectroyError/estimated.txt",Time_data);
  real_data=read_data("/home/n1/notes/Sophus/trajectroyError/groundtruth.txt",Time_real_data);
  cout<<Time_data[0]<<endl;
  cout<<real_data[0].matrix()<<endl;

  assert(!real_data.empty() && !data.empty());//如果数据为空终止程序执行
  assert(real_data.size() == data.size());//如果数据长度不相同终止程序执行
  double error[3]={0,0,0};
  double error_sum[3]={0,0,0};
  double rmse=0;//绝对轨迹误差
  double ATE=0;//绝对平移误差
  double RPE=0;//相对时间误差 10s内
  double Delta=0;//变化时间
  double Delta_length=0;//固定时间内数据长度
  for(size_t t=0;t<real_data.size();t++)//计算误差
  {
    Sophus::SE3d p2=real_data[t];
    Sophus::SE3d p1=data[t];
    error[0]=(p2.inverse()*p1).log().norm();//norm() 范数
    error[1]=(p2.inverse()*p1).translation().norm();//平移部分
   
    error_sum[0]+=error[0]*error[0];
    error_sum[1]+=error[1]*error[1];
    Delta=double(Time_data[t]-Time_data[0]);
    
    if(Delta<10)//10s内的相对位姿误差
    {
      p2=real_data[t].inverse()*real_data[t+1];
      p1=data[t].inverse()*data[t+1];
      error[2]+=(p2.inverse()*p1).log().norm();
      error_sum[2]+=error[2]*error[2];
      cout<<"error[2]:"<<error[2]<<endl;
      Delta_length++;
    }
  }
  
  rmse=double(sqrt(error_sum[0]/real_data.size()));
  ATE=double(sqrt(error_sum[1]/real_data.size()));
  RPE=double(sqrt(error_sum[2]/Delta_length));
  cout<<"RMSE:"<<rmse<<endl;
  cout<<"ATE:"<<ATE<<endl;
  cout<<"RPE:"<<RPE<<endl;
  plot_jectory(real_data,data);

  return 0;
}





//读取数据函数
TrajectoryType read_data(string path, vector<double> &Time)
{
  ifstream fin(path);
  TrajectoryType ject;
  if(!fin)
  {
    cout<<"error"<<endl;return ject;
  }
  double time,tx,ty,tz,qx,qy,qz,qw;
  while (!fin.eof())//处理读取到的数据
  {
    fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;//重定向数据到
    //cout<<"time"<<time<<endl;

    Sophus::SE3d p1(Eigen::Quaterniond(qx,qy,qz,qw),Vector3d(tx,ty,tz));//组成一个SE 欧氏旋转群
    Time.push_back(time);
    ject.push_back(p1);//保存数据点
  }
  cout<<"time"<<Time[0]<<endl;
  return ject;
}

//绘制点函数
void plot_jectory(TrajectoryType true_pose, TrajectoryType pose)
{
  int w=1024,h=768;
  pangolin::CreateWindowAndBind("Display",w,h);//创建名称，大小
  
  glEnable(GL_DEPTH_TEST);//景深
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);//开启颜色混合
  pangolin::OpenGlRenderState s_cam(//相机参数初始化
      pangolin::ProjectionMatrix(w, h, w/2, h/2, w/2, h/2, 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, -float(w/h))
      .SetHandler(new pangolin::Handler3D(s_cam));//创建显示界面
  while (!pangolin::ShouldQuit())
  {
    
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    
    for(int i=0;i<true_pose.size()-1;i++)//读取点数据，画直线
    {
      glColor3f(0,0,1);
      
      glBegin(GL_LINES);
      auto p1 = true_pose[i], p2 = true_pose[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<pose.size()-1;i++)//读取点数据，画直线
    {
      glColor3f(0,1,0);
      glBegin(GL_LINES);
      glVertex3f(pose[i].translation()[0],pose[i].translation()[1],pose[i].translation()[2]);
      glVertex3f(pose[i+1].translation()[0],pose[i+1].translation()[1],pose[i+1].translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    
  }
  
}