c++ OpenCV绘制SLAM轨迹(3D点)

最新推荐文章于 2025-04-02 15:26:23 发布

踏雪飞鸿Jaf

最新推荐文章于 2025-04-02 15:26:23 发布

阅读量3.5k

点赞数

分类专栏： C++ operation and syntax tool code

本文链接：https://blog.youkuaiyun.com/wq1psa78/article/details/103773469

版权

该博客介绍了如何使用C++和OpenCV读取包含SLAM位姿信息的文件，并绘制3D轨迹。代码示例展示了如何处理不同数据集的轨迹，并以红色和绿色区分不同轨迹。在本地运行时，可以依据CMakeLists.txt配置文件进行编译，参考了关于OpenCV viz模块的博客来实现轨迹可视化。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

下面代码主要功能:
- 读取保存有pose的文件,文件格式为:timestamp tx ty tz qw qx qy qz;
- 第3个参数dataset_i表示测试的第i个数据集的轨迹,代码中,直接将第i个数据集的groundtruth写死了.
- opencv绘制轨迹,用红色表示第一个文件中的轨迹,绿色表示第二个文件中的轨迹.

局部效果图:
opencv绘制SLAM轨迹

下面是代码:

//#include<opencv2/opencv.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include<fstream>
#include<iostream>
#include<vector>
#include<Eigen/Geometry>
#include<Eigen/Core>
 
using namespace std;
using namespace cv;
 
 
 
int main(int argc,char **argv)
{
    if(argc != 4){
        cerr << "sytax: ./drawTrajectory trajectory_vio.txt trajectory_viwo.txt dataset_index" << endl;
        return 0;
    }
    string path = argv[1];
    ifstream fin_vio(path.c_str());
    path = argv[2];
    ifstream fin_viwo(path.c_str());
    if (!fin_vio || !fin_viwo)
    {
        cerr << "error in openning the file !" << endl;
        return 0;
    }
    //groundtruth
    int dataset_i = atoi(argv[3]);
    double t_wcn_gt1[3] = {-0.232988  , 0.178259 , 0.00207278};
    double t_wcn_gt2[3] = {0.0099497 , 0.153633 , 0.0190607};

    // visualization
    cv::viz::Viz3d vis("Visual Odometry");
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
    vis.setBackgroundColor(cv::viz::Color::white());
 
    // draw the trace
    int i = 0;
    Point3d

最低0.47元/天解锁文章