用于SLAM的，如何比较两断轨迹，以及如何将这两段轨迹用pangolin在界面中绘制出来

用于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 << “rmse = “ << rmse << endl;

    // 把轨迹画出来
    DrawTrajectory（estimated_poses groundtruth_poses）;

    return 0;
}


// 读取位姿数据
void ReadPoses（vector> &poses string trajectory_file）
{
    ifstream fs（trajectory_file）;
    if（!fs.is_open（））
    {
        cout << “Error: cannot open file “ << trajectory_file << “ !“ << endl;
        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 << “Error! The size of estimated pose and ground truth pose does not match!“ << endl;
        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