軌跡繪製

SLAM問題的目標之一就是精確的估計相機運動的軌跡（姿態），如果我們將相機運動的軌跡繪製出來，就可以直觀的觀察它的運動是否符合預期

。給定一個軌跡文件trajectory.txt，該文件的每一行由若干個數據組成，格式為 [time, tx, ty, tz, qx, qy, qz, qw],其中 time 為時間，tx,ty,tz 為平移部分，qx,qy,qz,qw 是四元數表示的旋轉部分，請完成數據讀取部分的代碼，繪製部分代碼已經給出。 代碼框架和軌跡數據見：


#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

// path to trajectory file
string trajectory_file = "./trajectory.txt";

void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);

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

vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

/// implement pose reading code
// 開始你的代碼
/* //自己寫的
ifstream infile("./trajectory.txt",ios::in);;
std::string feature; //存儲讀取的每行數據
float feat_onePoint; //存儲每行按空格分開的每一個float數據
std::vector<float> lines; //存儲每行數據
//std::vector<vector<float>> lines_feat; //存儲所有數據
//lines_feat.clear();
poses.clear();
Eigen::Vector3d t;
Eigen::Quaterniond q;
while(!infile.eof())
{
getline(infile, feature); //一次讀取一行數據
stringstream stringin(feature); //使用串流實現對string的輸入輸出操作
lines.clear();
while (stringin >> feat_onePoint) { //按空格一次讀取一個數據存入feat_onePoint
lines.push_back(feat_onePoint); //存儲每行按空格分開的數據
}
for(auto i=0;i<lines.size();++i)
{
t<<lines[1],lines[2],lines[3];
q=Eigen::Quaterniond(lines[7],lines[4],lines[5],lines[6]);

}
//lines_feat.push_back(lines); //存儲所有數據
Sophus::SE3 SE3_qt(q,t);
poses.push_back(SE3_qt);
}
infile.close();*/

///網上的答案
fstream finfile;
finfile.open(trajectory_file.c_str());
if (!finfile.is_open()){
cout << "file is empty!" <<endl;
return -1;
}
string line;
double timestamp, tx, ty, tz, qx, qy, qz, qw;
while( getline(finfile,line) ){
stringstream lineStream(line);
lineStream>>timestamp>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
//if (timestamp == "#"){
// cout << "WARN: INF ERROR" << endl;
// continue;
//}
Eigen::Vector3d t(tx,ty,tz);
Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized();
Sophus::SE3 SE3_qt(q,t);
poses.push_back(SE3_qt);
}

// 結束你的代碼

// draw trajectory in pangolin
DrawTrajectory(poses);
return 0;
}

// 無需改動以下繪圖程序
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
if (poses.empty()) {
cerr << "Trajectory is empty!" << endl;
return;
}

// create pangolin window and plot the trajectory
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 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, -1024.0f / 768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));

while (pangolin::ShouldQuit() == false) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

d_cam.Activate(s_cam);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

glLineWidth(2);
for (size_t i = 0; i < poses.size() - 1; i++) {
glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);
}

}

好的參考：關於Sophus使用

最新布爾教育php最後一期學員(完整) - 碼農教程?

www.manongjc.com

網上答案

https://blog.csdn.net/xdzhangzhenhao/article/details/81462338?

blog.csdn.net