原文在这里
数据集格式为:时间+平移+旋转矩阵四元数。
#include<opencv2/opencv.hpp>
#include<fstream>
#include<iostream>
#include<vector>
#include<Eigen/Geometry>
#include<Eigen/Core>
using namespace std;
using namespace cv;
int main()
{
ifstream fin("KeyFrameTrajectory.txt");
if (!fin)
{
cerr << "error in openning the file !" << endl;
return 0;
}
// visualization
cv::viz::Viz3d vis("Visual Odometry");
cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
vis.setBackgroundColor(cv::viz::Color::black());
// draw the trace
int i = 0;
Point3d point_begin(0.0, 0.0, 0.0);
Point3d point_end;
//cv::viz::WLine wline(cv::Point3f(0, 0, 0), (100, 100, 100), cv::Scalar(0, 0, 255));
cv::Point3d cam_pos(0, -1.0, -1.0), cam_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);
cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir);
vis.setViewerPose(cam_pose);
world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);
vis.showWidget("World", world_coor);
vis.showWidget("Camera", camera_coor);
vector<int> count_color;
vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
vector<vector<double>> vecs;
for (int i = 0;; i++)
{
if (fin.eof())
{
cout << "read over " << endl;
break;
}
vector<double> vec(8,0); //获取每行数据
for (auto & d : vec)
{
fin >> d;//双向影响
}
vecs.push_back(vec);
}
if (false)
{
for (int j = 0; j < vecs.size(); j++)
{
for (int i = 1; i < 8; i++)// 获取位姿
{
cout << vecs[j][i] << " ";
}
cout << endl;
}
}
for (int j = 0; j < vecs.size(); j++)
{
Eigen::Quaterniond q(vecs[j][7], vecs[j][4], vecs[j][5], vecs[j][6]);
Eigen::Isometry3d T(q);
T.pretranslate(Eigen::Vector3d(vecs[j][1], vecs[j][2], vecs[j][3]));
poses.push_back(T);
cv::Affine3d M(
cv::Affine3d::Mat3(
T(0, 0), T(0, 1), T(0, 2),
T(1, 0), T(1, 1), T(1, 2),
T(2, 0), T(2, 1), T(2, 2)
),
cv::Affine3d::Vec3(
T.translation()(0, 0),
T.translation()(1, 0),
T.translation()(2, 0)
)
);
//cout << "x = " << T.translation()(0, 0) << " z = " << T.translation()(1, 0) << endl;
//*******************************************************************************************
// 画出轨迹
//*******************************************************************************************
vector<cv::viz::WLine> lines;
point_end = Point3d( // 更新终点
T.translation()(0, 0),
T.translation()(1, 0),
T.translation()(2, 0)
);
viz::WLine line(point_begin, point_end, cv::viz::Color::green());
lines.push_back(line); //收集 第一个 line 到 当前line
// 每个循环 画出 第一个 line 到 当前line
for (vector<cv::viz::WLine>::iterator iter = lines.begin(); iter != lines.end(); iter++)
{
string id = to_string(i);
vis.showWidget(id, *iter);
i++;
}
point_begin = point_end; // 更新 起始点
vis.setWidgetPose("Camera", M);
vis.spinOnce(1, false);
}
vis.saveScreenshot("KeyFrameTrajectory.png");
return 1;
}