91av视频/亚洲h视频/操亚洲美女/外国一级黄色毛片 - 国产三级三级三级三级

  • 大小: 118KB
    文件類型: .zip
    金幣: 2
    下載: 0 次
    發布日期: 2021-05-18
  • 語言: 其他
  • 標簽: SLAM??trajectory??c??

資源簡介

用于SLAM的,如何比較兩斷軌跡,以及如何將這兩段軌跡用pangolin在界面中繪制出來

資源截圖

代碼片段和文件信息

#include?
#include?
#include?
#include?
#include?

//?need?pangolin?for?plotting?trajectory
#include?

using?namespace?std;

//?path?to?trajectory?file
string?estimated_trajectory_file?=?“../data/estimated.txt“;
string?groundtruth_trajectory_file?=?“../data/groundtruth.txt“;


//?讀取位姿數據
void?ReadPoses(vector>?&poses?string?trajectory_file);

//?比較兩個?pose
double?CompareTwoPose(Sophus::SE3?estimated_pose?Sophus::SE3?groundtruth_pose);

//?比較兩組?pose
double?ComparePoses(vector>?estimated_poses
????????????????????vector>?groundtruth_poses);

//?function?for?plotting?trajectory?don‘t?edit?this?code
//?start?point?is?red?and?end?point?is?blue
void?DrawTrajectory(vector>);

//?重載一個繪圖函數
void?DrawTrajectory(vector>?estimated_poses
????????????????????vector>?groundtruth_poses);

int?main(int?argc?char?**argv)?{

????vector>?estimated_poses;
????vector>?groundtruth_poses;

????//?讀取位姿
????ReadPoses(estimated_poses?estimated_trajectory_file);
????ReadPoses(groundtruth_poses?groundtruth_trajectory_file);

????//?比較兩組位姿數據
????double?rmse(0);
????rmse?=?ComparePoses(estimated_poses?groundtruth_poses);

????cout?<
????//?把軌跡畫出來
????DrawTrajectory(estimated_poses?groundtruth_poses);

????return?0;
}


//?讀取位姿數據
void?ReadPoses(vector>?&poses?string?trajectory_file)
{
????ifstream?fs(trajectory_file);
????if(!fs.is_open())
????{
????????cout?<????????return;
????}
????string?sline;
????double?t?tx?ty?tz?qx?qy?qz?qw;
????while(getline(fs?sline))
????{
????????stringstream?ss(sline);
????????ss?>>?t?>>?tx?>>?ty?>>?tz?>>?qx?>>?qy?>>?qz?>>?qw;
????????Sophus::SE3?SE3(Eigen::Quaterniond(qw?qx?qy?qz)?Eigen::Vector3d(tx?ty?tz));
????????poses.push_back(SE3);
????}
????fs.close();
}


//?比較兩個?pose
double?CompareTwoPose(Sophus::SE3?estimated_pose?Sophus::SE3?groundtruth_pose)
{
????Sophus::SE3?error?=?estimated_pose.inverse()?*?groundtruth_pose;
????Sophus::Vector6d?se3?=?error.log();
????double?norm?=?se3.norm();
????return?norm*norm;
}


//?比較兩組?pose
double?ComparePoses(vector>?estimated_poses
????????????????????vector>?groundtruth_poses)
{
????if(estimated_poses.size()?!=?groundtruth_poses.size())
????{
????????cout?<????????return?-1;
????}
????d

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-07-10?20:22??ch3\
?????文件????????1461??2018-07-01?16:30??ch3\CMakeLists.txt
?????目錄???????????0??2018-07-10?20:08??ch3\data\
?????文件??????104075??2017-12-10?15:08??ch3\data\estimated.txt
?????文件??????103876??2017-12-10?15:08??ch3\data\groundtruth.txt
?????文件???????65386??2017-12-10?14:13??ch3\data\trajectory.txt
?????目錄???????????0??2018-06-16?10:54??ch3\include\
?????目錄???????????0??2018-06-16?10:54??ch3\src\
?????目錄???????????0??2018-07-10?20:22??ch3\test\
?????文件????????7078??2018-06-23?11:35??ch3\test\compare_trajectory.cpp
?????文件????????2872??2018-06-23?11:34??ch3\test\draw_trajectory.cpp

評論

共有 條評論