opencv - viz 画出SLAM轨迹

原文在这里

数据集格式为：时间+平移+旋转矩阵四元数。

#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;
}