1.效果图。
2.plotTrajectory.cpp
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>
// 本例演示了如何画出一个预先存储的轨迹
using namespace std;
using namespace Eigen;
// path to trajectory file
string trajectory_file = "../trajectory.txt";
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
int main(int argc, char **argv) {
vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
ifstream fin(trajectory_file);
if (!fin) {
cout << "cannot find trajectory file at " << trajectory_file << endl;
return 1;
}
while (!fin.eof()) {
double time, tx, ty, tz, qx, q