使用Pangolin对Visual SLAM结果可视化与评估

dockermyself

已于 2024-03-18 17:47:01 修改

阅读量674

点赞数 9

CC 4.0 BY-SA版权

文章标签：算法

于 2024-03-18 15:43:03 首次发布

本文链接：https://blog.youkuaiyun.com/DeepLearnerZxf/article/details/136811009

Visual SLAM结果可视化与评估

近年来，随着计算机视觉和机器学习技术的快速发展，同时以SLAM（Simultaneous Localization and Mapping）为代表的视觉定位与地图构建技术也取得了长足的进步。SLAM技术在自动驾驶、增强现实、无人机导航等领域具有广泛的应用前景。然而，要想确保SLAM系统的准确性和稳定性，就需要对其结果进行可视化和评估。

本文将介绍如何利用C++编写的Visual SLAM代码，实现对SLAM结果的可视化和评估，并展示在优快云博客中。我们将使用Pangolin库进行可视化，以及Sophus库处理SE(3)变换。

实验背景
在实验中，我们利用C++编写的Visual SLAM代码，对SLAM系统的输出进行评估。该代码可以读取SLAM系统输出的轨迹和地图数据，并与真实数据进行对比，计算出轨迹和地图的Root Mean Square Error（RMSE）。
实验步骤
数据准备

首先，我们需要准备SLAM系统输出的轨迹和地图数据，以及真实的轨迹和地图数据。这些数据通常保存在文本文件中，包括相机位姿和地图点的坐标。

数据读取与计算RMSE

接下来，我们利用C++代码读取SLAM系统输出的数据，并计算与真实数据之间的RMSE。这一步骤包括读取轨迹数据和地图数据，以及计算轨迹和地图的RMSE值。

结果可视化

最后，我们利用Pangolin库将SLAM系统输出的轨迹和地图可视化出来，并与真实数据进行对比。通过可视化，我们可以直观地观察到SLAM系统的性能表现，并对其进行评估。
相机位姿与三维点优化结果，蓝色姿态-初始值，红色姿态-优化后值，黑色点：真值，红色点：优化后值
在这里插入图片描述

实验结果
我们将通过博客文章展示实验结果，包括SLAM系统输出的轨迹和地图可视化图像，以及计算得到的RMSE值。通过对比可视化图像和RMSE值，我们可以评估SLAM系统的准确性和稳定性。
参考资料
Pangolin官方文档：Pangolin Documentation
Sophus官方文档：Sophus Documentation

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace Sophus;
using namespace std;

// 轨迹
string init_trajectory_file = "../data/init_trajectory.txt";
string opti_trajectory_file = "../data/opti_trajectory.txt";
// 三维点
string init_point_file = "../data/init_points.txt";
string opti_point_file = "../data/opti_points.txt";
void DrawCamera(const Eigen::Matrix4d &Tcw, Eigen::Vector3d color = Eigen::Vector3d(0, 1, 0))
{
   
   
    const float size = 0.1;
    glPushMatrix();
    glMultMatrixd(Tcw.data());
    // 绘制相机外轮廓
    glLineWidth(2);
    glBegin(GL_LINES);

    glColor3f(color[0], color[1], color[2]);
    glVertex3f(0, 0, 0);
    glVertex3f(size, size, size);

    glVertex3f(0, 0, 0);
    glVertex3f(size, -size, size);

    glVertex3f(0, 0, 0);
    glVertex3f(size, -size, -size);

    glVertex3f(0, 0, 0);
    glVertex3f(size, size, -size);

    glVertex3f(size, size, size);
    glVertex3f(size, -size, size);

    glVertex3f(size, -size, size);
    glVertex3f(size, -size, -size);

    glVertex3f(size, -size, -size);
    glVertex3f(size, size, -size);

    glVertex3f(size, size, -size);
    glVertex3f(size, size, size);
    glEnd();

    glPopMatrix();