#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