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

Visual SLAM结果可视化与评估

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

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

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

2. 实验步骤
   数据准备

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

3. 数据读取与计算RMSE

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

4. 结果可视化

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

5. 实验结果
   我们将通过博客文章展示实验结果，包括SLAM系统输出的轨迹和地图可视化图像，以及计算得到的RMSE值。通过对比可视化图像和RMSE值，我们可以评估SLAM系统的准确性和稳定性。

6. 参考资料
   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();
}

void DrawPoint(const Eigen::Vector3d &landmark,const Eigen::Vector3d color = Eigen::Vector3d(0, 1, 0))
{
    glPointSize(2);
    glBegin(GL_POINTS);
    glColor3f(color[0], color[1], color[2]);
    glVertex3d(landmark[0], landmark[1], landmark[2]);
    glEnd();
}

void DrawCoordinate()
{
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(0, 0, 0);
    glVertex3f(1, 0, 0);
    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 1, 0);
    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 1);
    glEnd();
}

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> LandmarkType;

TrajectoryType ReadTrajectory(const string &path)
{
    ifstream fin(path);
    TrajectoryType trajectory;
    if (!fin)
    {
        cerr << "trajectory " << path << " not found." << endl;
        return trajectory;
    }

    while (!fin.eof())
    {
        double tx, ty, tz, qx, qy, qz, qw;
        fin >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3d p(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
        trajectory.push_back(p);
    }
    return trajectory;
}

LandmarkType ReadLandmark(const string &path)
{
    ifstream fin(path);
    LandmarkType landmarks;
    if (!fin)
    {
        cerr << "landmark " << path << " not found." << endl;
        return landmarks;
    }

    while (!fin.eof())
    {
        double x, y, z;
        fin >> x >> y >> z;
        Eigen::Vector3d p(x, y, z);
        landmarks.push_back(p);
    }
    return landmarks;
}

void DrawTrajectoryAndLandmark(const TrajectoryType &init_trajectory,
                               const TrajectoryType &opti_trajectory,
                               const LandmarkType &init_landmarks,
                               const LandmarkType &opti_landmarks)
{
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    //  fx = 720, fy = 720, cx = 640, cy = 360;
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 720, 720, 640, 360, 0.1, 1000),
        pangolin::ModelViewLookAt(0, 0, -1, 0, 0, 0, pangolin::AxisY));

    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < init_trajectory.size() - 1; i++)
        {
            DrawCamera(init_trajectory[i].matrix(), Eigen::Vector3d(0, 0, 1));
            glColor3f(0.0f, 0.0f, 1.0f); 
            glBegin(GL_LINES);
            auto p1 = init_trajectory[i], p2 = init_trajectory[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for (size_t i = 0; i < opti_trajectory.size() - 1; i++)
        {
            DrawCamera(opti_trajectory[i].matrix(), Eigen::Vector3d(1, 0, 0));
            glColor3f(1.0f, 0.0f, 0.0f); 
            glBegin(GL_LINES);
            auto p1 = opti_trajectory[i], p2 = opti_trajectory[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        for(size_t i = 0; i < init_landmarks.size(); i++)
        {
            DrawPoint(init_landmarks[i], Eigen::Vector3d(0, 0, 1));// blue
        }

        for(size_t i = 0; i < opti_landmarks.size(); i++)
        {
            DrawPoint(opti_landmarks[i], Eigen::Vector3d(1, 0, 0));// red
        }
        // 坐标轴
        pangolin::glDrawAxis(0.1);
        pangolin::FinishFrame();
        usleep(5000); // sleep 5 ms
    }
}

int main(int argc, char **argv)
{
    TrajectoryType init_trajectory = ReadTrajectory(init_trajectory_file);
    TrajectoryType opti_trajectory = ReadTrajectory(opti_trajectory_file);
    assert(!init_trajectory.empty() && !opti_trajectory.empty());
    assert(init_trajectory.size() == opti_trajectory.size());
    LandmarkType init_landmarks = ReadLandmark(init_point_file);
    LandmarkType opti_landmarks = ReadLandmark(opti_point_file);
    assert(!init_landmarks.empty() && !opti_landmarks.empty());
    assert(init_landmarks.size() == opti_landmarks.size());
    

    // compute rmse
    double rmse_trajectory = 0;
    for (size_t i = 0; i < opti_trajectory.size(); i++)
    {
        Sophus::SE3d p1 = opti_trajectory[i], p2 = init_trajectory[i];
        double error = (p2.inverse() * p1).log().norm();
        rmse_trajectory += error * error;
    }
    rmse_trajectory = rmse_trajectory / double(opti_trajectory.size());
    rmse_trajectory = sqrt(rmse_trajectory);
    cout << "Trajectory RMSE = " << rmse_trajectory << endl;

    double rmse_landmark = 0;
    for(size_t i = 0; i < opti_landmarks.size(); i++)
    {
        Eigen::Vector3d p1 = opti_landmarks[i], p2 = init_landmarks[i];
        double error = (p2 - p1).norm();
        rmse_landmark += error * error;
    }
    rmse_landmark = rmse_landmark / double(opti_landmarks.size());
    rmse_landmark = sqrt(rmse_landmark);
    cout << "Landmark RMSE = " << rmse_landmark << endl;

    DrawTrajectoryAndLandmark(init_trajectory, opti_trajectory, init_landmarks, opti_landmarks);
    return 0;
}