当前位置:   article > 正文

slam十四讲评价轨迹的误差(trajectoryError)_median trajectory error

median trajectory error

trajectoryError.cpp

  1. #include <sophus/se3.hpp>
  2. #include <unistd.h>
  3. #include <string>
  4. #include <iostream>
  5. #include <fstream>
  6. #include <cmath>
  7. #include <pangolin/pangolin.h>
  8. #include <Eigen/Core>
  9. #include <Eigen/Geometry>
  10. using namespace std;
  11. using namespace Eigen;
  12. void ReadData(string FileName ,vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses);
  13. double ErrorTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g,
  14. vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e);
  15. void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g,
  16. vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e);
  17. int main(int argc, char **argv)
  18. {
  19. string GroundFile = "../groundtruth.txt";
  20. string ErrorFile = "../estimated.txt";
  21. double trajectory_error_RMSE = 0;
  22. vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_g;
  23. vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_e;
  24. ReadData(GroundFile,poses_g);
  25. ReadData(ErrorFile,poses_e);
  26. trajectory_error_RMSE = Error
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/weixin_40725706/article/detail/710981
推荐阅读
相关标签
  

闽ICP备14008679号