opencv - viz 画出SLAM轨迹

opencv画图：首先创建一个窗口，然后show你想要添加的widget，问题是：部件widget在每一帧中只能显示当前状态，对于历史状态全部会清除，所以每一次循环，你不仅要添加widget的当前状态，还要将历史状态全部添加进去（第n次循环要实例化n个widget）；

 

  1 #include<opencv2/opencv.hpp>
  2 
  3 #include<fstream>
  4 #include<iostream>
  5 #include<vector>
  6 #include<Eigen/Geometry>
  7 #include<Eigen/Core>
  8 
  9 using namespace std;
 10 using namespace cv;
 11 
 12 
 13 
 14 int main()
 15 {
 16     ifstream fin("KeyFrameTrajectory.txt");
 17     if (!fin)
 18     {
 19         cerr << "error in openning the file !" << endl;
 20         return 0;
 21     }
 22 
 23     // visualization
 24     cv::viz::Viz3d vis("Visual Odometry");
 25     cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
 26     vis.setBackgroundColor(cv::viz::Color::black());
 27 
 28     // draw the trace
 29     int i = 0;
 30     Point3d point_begin(0.0, 0.0, 0.0);
 31     Point3d point_end;
 32 
 33     //cv::viz::WLine wline(cv::Point3f(0, 0, 0), (100, 100, 100), cv::Scalar(0, 0, 255));
 34     cv::Point3d cam_pos(0, -1.0, -1.0), cam_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);
 35     cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir);
 36     vis.setViewerPose(cam_pose);
 37 
 38     world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
 39     camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);
 40     vis.showWidget("World", world_coor);
 41     vis.showWidget("Camera", camera_coor);
 42 
 43     vector<int> count_color;
 44     vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
 45     vector<vector<double>> vecs;
 46     for (int i = 0;; i++)
 47     {
 48         if (fin.eof())
 49         {
 50             cout << "read over " << endl;
 51             break;
 52         }
 53 
 54         vector<double> vec(8,0); //获取每行数据
 55         for (auto & d : vec)
 56         {
 57             fin >> d;//双向影响
 58         }
 59         vecs.push_back(vec);
 60     }
 61     if (false)
 62     {
 63         for (int j = 0; j < vecs.size(); j++)
 64         {
 65             for (int i = 1; i < 8; i++)// 获取位姿
 66             {
 67                 cout << vecs[j][i] << "  ";
 68             }
 69             cout << endl;
 70         }
 71     }
 72     
 73     
 74     for (int j = 0; j < vecs.size(); j++)
 75     {
 76         Eigen::Quaterniond q(vecs[j][7], vecs[j][4], vecs[j][5], vecs[j][6]);
 77         Eigen::Isometry3d T(q);
 78         T.pretranslate(Eigen::Vector3d(vecs[j][1], vecs[j][2], vecs[j][3]));
 79         poses.push_back(T);
 80         cv::Affine3d M(
 81             cv::Affine3d::Mat3(
 82                 T(0, 0), T(0, 1), T(0, 2),
 83                 T(1, 0), T(1, 1), T(1, 2),
 84                 T(2, 0), T(2, 1), T(2, 2)
 85             ),
 86             cv::Affine3d::Vec3(
 87                 T.translation()(0, 0), 
 88                 T.translation()(1, 0),
 89                 T.translation()(2, 0)
 90             )
 91         );
 92         //cout << "x = " << T.translation()(0, 0) << " z = " << T.translation()(1, 0) << endl;
 93         
 94         //*******************************************************************************************
 95         // 画出轨迹
 96         //*******************************************************************************************
 97         vector<cv::viz::WLine> lines; 
 98         point_end = Point3d( // 更新终点
 99             T.translation()(0, 0),
100             T.translation()(1, 0),
101             T.translation()(2, 0)
102             );
103 
104         viz::WLine line(point_begin, point_end, cv::viz::Color::green());
105         lines.push_back(line); //收集  第一个 line 到 当前line
106         // 每个循环 画出 第一个 line 到 当前line
107         for (vector<cv::viz::WLine>::iterator iter = lines.begin(); iter != lines.end(); iter++)
108         {
109             string id = to_string(i);
110             vis.showWidget(id, *iter);
111             i++;
112         }
113         point_begin = point_end; // 更新 起始点
114 
115         vis.setWidgetPose("Camera", M);
116         vis.spinOnce(1, false);
117     }
118     vis.saveScreenshot("KeyFrameTrajectory.png");
119     return 1;
120 }

转载于:https://www.cnblogs.com/winslam/p/9244598.html